From 53225a0990beb7ff908d101cf6aabcce24208cf3 Mon Sep 17 00:00:00 2001 From: yikestone Date: Thu, 11 Jan 2018 01:55:19 +0530 Subject: [PATCH] One Major Bug Fix and separate PIDs for the two motors. --- README.md | 3 + .../MegaRobogaiaPololu/MegaRobogaiaPololu.ino | 341 ------------------ .../libraries/MegaRobogaiaPololu/commands.h | 22 -- .../MegaRobogaiaPololu/diff_controller.h | 71 ---- .../libraries/MegaRobogaiaPololu/sensors.h | 34 -- .../src/libraries/MegaRobogaiaPololu/servos.h | 16 - .../ROSArduinoBridge/ROSArduinoBridge.ino | 242 +++---------- .../src/libraries/ROSArduinoBridge/commands.h | 13 +- .../ROSArduinoBridge/diff_controller.h | 68 +--- .../ROSArduinoBridge/encoder_driver.h | 10 - .../ROSArduinoBridge/encoder_driver.ino | 39 -- .../libraries/ROSArduinoBridge/motor_driver.h | 6 - .../ROSArduinoBridge/motor_driver.ino | 75 +--- .../src/libraries/ROSArduinoBridge/sensors.h | 34 -- .../src/libraries/ROSArduinoBridge/servos.h | 44 --- .../src/libraries/ROSArduinoBridge/servos.ino | 78 ---- ros_arduino_python/config/arduino_params.yaml | 13 +- .../src/ros_arduino_python/arduino_driver.py | 6 +- .../src/ros_arduino_python/base_controller.py | 36 +- 19 files changed, 107 insertions(+), 1044 deletions(-) delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h delete mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h delete mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h delete mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino diff --git a/README.md b/README.md index e162be1..6feb207 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,6 @@ +This Fork is modified for L298N motor driver and quad encoders, with one major bug fix and other minor bug fixes. +This supports separate PID control system for two (left, right) motors. + Overview -------- This branch (indigo-devel) is intended for ROS Indigo and above, and uses the Catkin buildsystem. It may also be compatible with ROS Hydro. diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino deleted file mode 100644 index 0f66429..0000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino +++ /dev/null @@ -1,341 +0,0 @@ -/********************************************************************* - * ROSArduinoBridge - - A set of simple serial commands to control a differential drive - robot and receive back sensor and odometry data. Default - configuration assumes use of an Arduino Mega + Pololu motor - controller shield + Robogaia Mega Encoder shield. Edit the - readEncoder() and setMotorSpeed() wrapper functions if using - different motor controller or encoder method. - - Created for the Pi Robot Project: http://www.pirobot.org - - Inspired and modeled after the ArbotiX driver by Michael Ferguson - - Software License Agreement (BSD License) - - Copyright (c) 2012, Patrick Goebel. - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above - copyright notice, this list of conditions and the following - disclaimer in the documentation and/or other materials provided - with the distribution. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#define USE_BASE // Enable the base controller code -//#undef USE_BASE // Disable the base controller code - -//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h -#undef USE_SERVOS // Disable use of PWM servos - -/* Serial port baud rate */ -#define BAUDRATE 57600 - -/* Maximum PWM signal */ -#define MAX_PWM 255 - -#if defined(ARDUINO) && ARDUINO >= 100 -#include "Arduino.h" -#else -#include "WProgram.h" -#endif - -/* Include definition of serial commands */ -#include "commands.h" - -/* Sensor functions */ -#include "sensors.h" - -/* Include servo support if required */ -#ifdef USE_SERVOS - #include - #include "servos.h" -#endif - -#ifdef USE_BASE - /* The Pololu motor driver shield */ - #include "DualVNH5019MotorShield.h" - - /* The Robogaia Mega Encoder shield */ - #include "MegaEncoderCounter.h" - - /* Create the motor driver object */ - DualVNH5019MotorShield drive; - - /* Create the encoder shield object */ - MegaEncoderCounter encoders(4); // Initializes the Mega Encoder Counter in the 4X Count mode - - /* PID parameters and functions */ - #include "diff_controller.h" - - /* Run the PID loop at 30 times per second */ - #define PID_RATE 30 // Hz - - /* Convert the rate into an interval */ - const int PID_INTERVAL = 1000 / PID_RATE; - - /* Track the next time we make a PID calculation */ - unsigned long nextPID = PID_INTERVAL; - - /* Stop the robot if it hasn't received a movement command - in this number of milliseconds */ - #define AUTO_STOP_INTERVAL 2000 - long lastMotorCommand = AUTO_STOP_INTERVAL; - - /* Wrap the encoder reading function */ - long readEncoder(int i) { - if (i == LEFT) return encoders.YAxisGetCount(); - else return encoders.XAxisGetCount(); - } - - /* Wrap the encoder reset function */ - void resetEncoder(int i) { - if (i == LEFT) return encoders.YAxisReset(); - else return encoders.XAxisReset(); - } - - /* Wrap the encoder reset function */ - void resetEncoders() { - resetEncoder(LEFT); - resetEncoder(RIGHT); - } - - /* Wrap the motor driver initialization */ - void initMotorController() { - drive.init(); - } - - /* Wrap the drive motor set speed function */ - void setMotorSpeed(int i, int spd) { - if (i == LEFT) drive.setM1Speed(spd); - else drive.setM2Speed(spd); - } - - // A convenience function for setting both motor speeds - void setMotorSpeeds(int leftSpeed, int rightSpeed) { - setMotorSpeed(LEFT, leftSpeed); - setMotorSpeed(RIGHT, rightSpeed); - } -#endif - -/* Variable initialization */ - -// A pair of varibles to help parse serial commands (thanks Fergs) -int arg = 0; -int index = 0; - -// Variable to hold an input character -char chr; - -// Variable to hold the current single-character command -char cmd; - -// Character arrays to hold the first and second arguments -char argv1[16]; -char argv2[16]; - -// The arguments converted to integers -long arg1; -long arg2; - -/* Clear the current command parameters */ -void resetCommand() { - cmd = NULL; - memset(argv1, 0, sizeof(argv1)); - memset(argv2, 0, sizeof(argv2)); - arg1 = 0; - arg2 = 0; - arg = 0; - index = 0; -} - -/* Run a command. Commands are defined in commands.h */ -int runCommand() { - int i = 0; - char *p = argv1; - char *str; - int pid_args[4]; - arg1 = atoi(argv1); - arg2 = atoi(argv2); - - switch(cmd) { - case GET_BAUDRATE: - Serial.println(BAUDRATE); - break; - case ANALOG_READ: - Serial.println(analogRead(arg1)); - break; - case DIGITAL_READ: - Serial.println(digitalRead(arg1)); - break; - case ANALOG_WRITE: - analogWrite(arg1, arg2); - Serial.println("OK"); - break; - case DIGITAL_WRITE: - if (arg2 == 0) digitalWrite(arg1, LOW); - else if (arg2 == 1) digitalWrite(arg1, HIGH); - Serial.println("OK"); - break; - case PIN_MODE: - if (arg2 == 0) pinMode(arg1, INPUT); - else if (arg2 == 1) pinMode(arg1, OUTPUT); - Serial.println("OK"); - break; - case PING: - Serial.println(Ping(arg1)); - break; -#ifdef USE_SERVOS - case SERVO_WRITE: - servos[arg1].write(arg2); - Serial.println("OK"); - break; - case SERVO_READ: - Serial.println(servos[arg1].read()); - break; -#endif - -#ifdef USE_BASE - case READ_ENCODERS: - Serial.print(readEncoder(LEFT)); - Serial.print(" "); - Serial.println(readEncoder(RIGHT)); - break; - case RESET_ENCODERS: - resetEncoders(); - Serial.println("OK"); - break; - case MOTOR_SPEEDS: - /* Reset the auto stop timer */ - lastMotorCommand = millis(); - if (arg1 == 0 && arg2 == 0) { - setMotorSpeeds(0, 0); - moving = 0; - } - else moving = 1; - leftPID.TargetTicksPerFrame = arg1; - rightPID.TargetTicksPerFrame = arg2; - Serial.println("OK"); - break; - case UPDATE_PID: - while ((str = strtok_r(p, ":", &p)) != '\0') { - pid_args[i] = atoi(str); - i++; - } - Kp = pid_args[0]; - Kd = pid_args[1]; - Ki = pid_args[2]; - Ko = pid_args[3]; - Serial.println("OK"); - break; -#endif - default: - Serial.println("Invalid Command"); - break; - } -} - -/* Setup function--runs once at startup. */ -void setup() { - Serial.begin(BAUDRATE); - -// Initialize the motor controller if used */ -#ifdef USE_BASE - initMotorController(); -#endif - -/* Attach servos if used */ -#ifdef USE_SERVOS - int i; - for (i = 0; i < N_SERVOS; i++) { - servos[i].attach(servoPins[i]); - } -#endif -} - -/* Enter the main loop. Read and parse input from the serial port - and run any valid commands. Run a PID calculation at the target - interval and check for auto-stop conditions. -*/ -void loop() { - while (Serial.available() > 0) { - - // Read the next character - chr = Serial.read(); - - // Terminate a command with a CR - if (chr == 13) { - if (arg == 1) argv1[index] = NULL; - else if (arg == 2) argv2[index] = NULL; - runCommand(); - resetCommand(); - } - // Use spaces to delimit parts of the command - else if (chr == ' ') { - // Step through the arguments - if (arg == 0) arg = 1; - else if (arg == 1) { - argv1[index] = NULL; - arg = 2; - index = 0; - } - continue; - } - else { - if (arg == 0) { - // The first arg is the single-letter command - cmd = chr; - } - else if (arg == 1) { - // Subsequent arguments can be more than one character - argv1[index] = chr; - index++; - } - else if (arg == 2) { - argv2[index] = chr; - index++; - } - } - } - -// If we are using base control, run a PID calculation at the appropriate intervals -#ifdef USE_BASE - if (millis() > nextPID) { - updatePID(); - nextPID += PID_INTERVAL; - } - - // Check to see if we have exceeded the auto-stop interval - if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {; - setMotorSpeeds(0, 0); - moving = 0; - } - -#endif -} - - - - - - diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h deleted file mode 100644 index 18ff0ab..0000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h +++ /dev/null @@ -1,22 +0,0 @@ -/* Define single-letter commands that will be sent by the PC over the - serial link. -*/ - -#define ANALOG_READ 'a' -#define GET_BAUDRATE 'b' -#define PIN_MODE 'c' -#define DIGITAL_READ 'd' -#define READ_ENCODERS 'e' -#define MOTOR_SPEEDS 'm' -#define PING 'p' -#define RESET_ENCODERS 'r' -#define SERVO_WRITE 's' -#define SERVO_READ 't' -#define UPDATE_PID 'u' -#define DIGITAL_WRITE 'w' -#define ANALOG_WRITE 'x' -#define LEFT 0 -#define RIGHT 1 - - - diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h deleted file mode 100644 index 0284681..0000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h +++ /dev/null @@ -1,71 +0,0 @@ -/* Functions and type-defs for PID control. - - Taken mostly from Mike Ferguson's ArbotiX code which lives at: - - http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/ -*/ - -/* PID setpoint info For a Motor */ -typedef struct { - double TargetTicksPerFrame; // target speed in ticks per frame - long Encoder; // encoder count - long PrevEnc; // last encoder count - int PrevErr; // last error - int Ierror; // integrated error - int output; // last motor setting -} -SetPointInfo; - -SetPointInfo leftPID, rightPID; - -/* PID Parameters */ -int Kp = 20; -int Kd = 12; -int Ki = 0; -int Ko = 50; - -unsigned char moving = 0; // is the base in motion? - -/* PID routine to compute the next motor commands */ -void doPID(SetPointInfo * p) { - long Perror; - long output; - - Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc); - - // Derivative error is the delta Perror - output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko; - p->PrevErr = Perror; - p->PrevEnc = p->Encoder; - - output += p->output; - // Accumulate Integral error *or* Limit output. - // Stop accumulating when output saturates - if (output >= MAX_PWM) - output = MAX_PWM; - else if (output <= -MAX_PWM) - output = -MAX_PWM; - else - p->Ierror += Perror; - - p->output = output; -} - -/* Read the encoder values and call the PID routine */ -void updatePID() { - /* Read the encoders */ - leftPID.Encoder = readEncoder(0); - rightPID.Encoder = readEncoder(1); - - /* If we're not moving there is nothing more to do */ - if (!moving) - return; - - /* Compute PID update for each motor */ - doPID(&rightPID); - doPID(&leftPID); - - /* Set the motor speeds accordingly */ - setMotorSpeeds(leftPID.output, rightPID.output); -} - diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h deleted file mode 100644 index 75caeb7..0000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h +++ /dev/null @@ -1,34 +0,0 @@ -/* Functions for various sensor types */ - -float microsecondsToCm(long microseconds) -{ - // The speed of sound is 340 m/s or 29 microseconds per cm. - // The ping travels out and back, so to find the distance of the - // object we take half of the distance travelled. - return microseconds / 29 / 2; -} - -long Ping(int pin) { - long duration, range; - - // The PING))) is triggered by a HIGH pulse of 2 or more microseconds. - // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: - pinMode(pin, OUTPUT); - digitalWrite(pin, LOW); - delayMicroseconds(2); - digitalWrite(pin, HIGH); - delayMicroseconds(5); - digitalWrite(pin, LOW); - - // The same pin is used to read the signal from the PING))): a HIGH - // pulse whose duration is the time (in microseconds) from the sending - // of the ping to the reception of its echo off of an object. - pinMode(pin, INPUT); - duration = pulseIn(pin, HIGH); - - // convert the time into meters - range = microsecondsToCm(duration); - - return(range); -} - diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h deleted file mode 100644 index 6100723..0000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h +++ /dev/null @@ -1,16 +0,0 @@ -/* Define the attachment of any servos here. - The example shows two servos attached on pins 3 and 5. -*/ - -#define N_SERVOS 2 - -Servo servos [N_SERVOS]; -byte servoPins [N_SERVOS] = {3, 5}; - - - - - - - - diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 3348af5..b4d5b24 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -1,78 +1,4 @@ -/********************************************************************* - * ROSArduinoBridge - - A set of simple serial commands to control a differential drive - robot and receive back sensor and odometry data. Default - configuration assumes use of an Arduino Mega + Pololu motor - controller shield + Robogaia Mega Encoder shield. Edit the - readEncoder() and setMotorSpeed() wrapper functions if using - different motor controller or encoder method. - - Created for the Pi Robot Project: http://www.pirobot.org - and the Home Brew Robotics Club (HBRC): http://hbrobotics.org - - Authors: Patrick Goebel, James Nugen - - Inspired and modeled after the ArbotiX driver by Michael Ferguson - - Software License Agreement (BSD License) - - Copyright (c) 2012, Patrick Goebel. - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above - copyright notice, this list of conditions and the following - disclaimer in the documentation and/or other materials provided - with the distribution. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -//#define USE_BASE // Enable the base controller code -#undef USE_BASE // Disable the base controller code - -/* Define the motor controller and encoder library you are using */ -#ifdef USE_BASE - /* The Pololu VNH5019 dual motor driver shield */ - #define POLOLU_VNH5019 - - /* The Pololu MC33926 dual motor driver shield */ - //#define POLOLU_MC33926 - - /* The RoboGaia encoder shield */ - #define ROBOGAIA - - /* Encoders directly attached to Arduino board */ - //#define ARDUINO_ENC_COUNTER - - /* L298 Motor driver*/ - //#define L298_MOTOR_DRIVER -#endif - -#define USE_SERVOS // Enable use of PWM servos as defined in servos.h -//#undef USE_SERVOS // Disable use of PWM servos - -/* Serial port baud rate */ #define BAUDRATE 57600 - -/* Maximum PWM signal */ #define MAX_PWM 255 #if defined(ARDUINO) && ARDUINO >= 100 @@ -80,65 +6,24 @@ #else #include "WProgram.h" #endif - -/* Include definition of serial commands */ #include "commands.h" - -/* Sensor functions */ -#include "sensors.h" - -/* Include servo support if required */ -#ifdef USE_SERVOS - #include - #include "servos.h" -#endif - -#ifdef USE_BASE - /* Motor driver function definitions */ - #include "motor_driver.h" - - /* Encoder driver function definitions */ - #include "encoder_driver.h" - - /* PID parameters and functions */ - #include "diff_controller.h" - - /* Run the PID loop at 30 times per second */ - #define PID_RATE 30 // Hz - - /* Convert the rate into an interval */ - const int PID_INTERVAL = 1000 / PID_RATE; - - /* Track the next time we make a PID calculation */ - unsigned long nextPID = PID_INTERVAL; - - /* Stop the robot if it hasn't received a movement command - in this number of milliseconds */ - #define AUTO_STOP_INTERVAL 2000 - long lastMotorCommand = AUTO_STOP_INTERVAL; -#endif - -/* Variable initialization */ - -// A pair of varibles to help parse serial commands (thanks Fergs) +#include "motor_driver.h" +#include "encoder_driver.h" +#include "diff_controller.h" +#define PID_RATE 30 // Hz +const int PID_INTERVAL = 1000 / PID_RATE; +unsigned long nextPID = PID_INTERVAL; +#define AUTO_STOP_INTERVAL 2000 +long lastMotorCommand = AUTO_STOP_INTERVAL; int arg = 0; int index = 0; - -// Variable to hold an input character char chr; - -// Variable to hold the current single-character command char cmd; - -// Character arrays to hold the first and second arguments char argv1[16]; char argv2[16]; - -// The arguments converted to integers long arg1; long arg2; -/* Clear the current command parameters */ void resetCommand() { cmd = NULL; memset(argv1, 0, sizeof(argv1)); @@ -149,7 +34,6 @@ void resetCommand() { index = 0; } -/* Run a command. Commands are defined in commands.h */ int runCommand() { int i = 0; char *p = argv1; @@ -162,40 +46,7 @@ int runCommand() { case GET_BAUDRATE: Serial.println(BAUDRATE); break; - case ANALOG_READ: - Serial.println(analogRead(arg1)); - break; - case DIGITAL_READ: - Serial.println(digitalRead(arg1)); - break; - case ANALOG_WRITE: - analogWrite(arg1, arg2); - Serial.println("OK"); - break; - case DIGITAL_WRITE: - if (arg2 == 0) digitalWrite(arg1, LOW); - else if (arg2 == 1) digitalWrite(arg1, HIGH); - Serial.println("OK"); - break; - case PIN_MODE: - if (arg2 == 0) pinMode(arg1, INPUT); - else if (arg2 == 1) pinMode(arg1, OUTPUT); - Serial.println("OK"); - break; - case PING: - Serial.println(Ping(arg1)); - break; -#ifdef USE_SERVOS - case SERVO_WRITE: - servos[arg1].setTargetPosition(arg2); - Serial.println("OK"); - break; - case SERVO_READ: - Serial.println(servos[arg1].getServo().read()); - break; -#endif -#ifdef USE_BASE case READ_ENCODERS: Serial.print(readEncoder(LEFT)); Serial.print(" "); @@ -217,20 +68,43 @@ int runCommand() { else moving = 1; leftPID.TargetTicksPerFrame = arg1; rightPID.TargetTicksPerFrame = arg2; - Serial.println("OK"); + + Serial.println("OK"); break; - case UPDATE_PID: + + case UPDATE_PID_L: while ((str = strtok_r(p, ":", &p)) != '\0') { pid_args[i] = atoi(str); i++; } - Kp = pid_args[0]; - Kd = pid_args[1]; - Ki = pid_args[2]; - Ko = pid_args[3]; + leftPID.Kp = pid_args[0]; + leftPID.Kd = pid_args[1]; + leftPID.Ki = pid_args[2]; + leftPID.Ko = pid_args[3]; + Serial.println("OK"); + break; + case UPDATE_PID_R: + while ((str = strtok_r(p, ":", &p)) != '\0') { + pid_args[i] = atoi(str); + i++; + } + rightPID.Kp = pid_args[0]; + rightPID.Kd = pid_args[1]; + rightPID.Ki = pid_args[2]; + rightPID.Ko = pid_args[3]; + Serial.println("OK"); + break; + case DISP_PID_P: + Serial.println(leftPID.Kp); + Serial.println(leftPID.Kd); + Serial.println(leftPID.Ki); + Serial.println(leftPID.Ko); + Serial.println(rightPID.Kp); + Serial.println(rightPID.Kd); + Serial.println(rightPID.Ki); + Serial.println(rightPID.Ko); Serial.println("OK"); break; -#endif default: Serial.println("Invalid Command"); break; @@ -241,10 +115,6 @@ int runCommand() { void setup() { Serial.begin(BAUDRATE); -// Initialize the motor controller if used */ -#ifdef USE_BASE - #ifdef ARDUINO_ENC_COUNTER - //set as inputs DDRD &= ~(1< 0) { // Read the next character @@ -324,9 +182,7 @@ void loop() { } } } - -// If we are using base control, run a PID calculation at the appropriate intervals -#ifdef USE_BASE + if (millis() > nextPID) { updatePID(); nextPID += PID_INTERVAL; @@ -337,14 +193,6 @@ void loop() { setMotorSpeeds(0, 0); moving = 0; } -#endif - -// Sweep servos -#ifdef USE_SERVOS - int i; - for (i = 0; i < N_SERVOS; i++) { - servos[i].doSweep(); - } -#endif + } diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h index a1c6672..eb61f59 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h @@ -5,19 +5,14 @@ #ifndef COMMANDS_H #define COMMANDS_H -#define ANALOG_READ 'a' #define GET_BAUDRATE 'b' -#define PIN_MODE 'c' -#define DIGITAL_READ 'd' #define READ_ENCODERS 'e' #define MOTOR_SPEEDS 'm' -#define PING 'p' #define RESET_ENCODERS 'r' -#define SERVO_WRITE 's' -#define SERVO_READ 't' -#define UPDATE_PID 'u' -#define DIGITAL_WRITE 'w' -#define ANALOG_WRITE 'x' +#define UPDATE_PID_L 'L' +#define UPDATE_PID_R 'R' +#define DISP_PID_P 'z' + #define LEFT 0 #define RIGHT 1 diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h index f648209..335524a 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h @@ -1,30 +1,16 @@ -/* Functions and type-defs for PID control. - Taken mostly from Mike Ferguson's ArbotiX code which lives at: - - http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/ -*/ - -/* PID setpoint info For a Motor */ typedef struct { double TargetTicksPerFrame; // target speed in ticks per frame long Encoder; // encoder count long PrevEnc; // last encoder count - /* - * Using previous input (PrevInput) instead of PrevError to avoid derivative kick, - * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ - */ int PrevInput; // last input - //int PrevErr; // last error - - /* - * Using integrated term (ITerm) instead of integrated error (Ierror), - * to allow tuning changes, - * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ - */ - //int Ierror; + int ITerm; //integrated term + int Kp = 20; + int Kd = 12; + int Ki = 0; + int Ko = 50; long output; // last motor setting } @@ -32,22 +18,9 @@ SetPointInfo; SetPointInfo leftPID, rightPID; -/* PID Parameters */ -int Kp = 20; -int Kd = 12; -int Ki = 0; -int Ko = 50; unsigned char moving = 0; // is the base in motion? -/* -* Initialize PID variables to zero to prevent startup spikes -* when turning PID on to start moving -* In particular, assign both Encoder and PrevEnc the current encoder value -* See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/ -* Note that the assumption here is that PID is only turned on -* when going from stop to moving, that's why we can init everything on zero. -*/ void resetPID(){ leftPID.TargetTicksPerFrame = 0.0; leftPID.Encoder = readEncoder(LEFT); @@ -64,7 +37,6 @@ void resetPID(){ rightPID.ITerm = 0; } -/* PID routine to compute the next motor commands */ void doPID(SetPointInfo * p) { long Perror; long output; @@ -74,29 +46,18 @@ void doPID(SetPointInfo * p) { input = p->Encoder - p->PrevEnc; Perror = p->TargetTicksPerFrame - input; - - /* - * Avoid derivative kick and allow tuning changes, - * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ - * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ - */ - //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko; - // p->PrevErr = Perror; - output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko; + output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko); p->PrevEnc = p->Encoder; output += p->output; - // Accumulate Integral error *or* Limit output. - // Stop accumulating when output saturates + if (output >= MAX_PWM) output = MAX_PWM; else if (output <= -MAX_PWM) output = -MAX_PWM; else - /* - * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ - */ - p->ITerm += Ki * Perror; + + p->ITerm += (p->Ki) * Perror; p->output = output; p->PrevInput = input; @@ -108,23 +69,14 @@ void updatePID() { leftPID.Encoder = readEncoder(LEFT); rightPID.Encoder = readEncoder(RIGHT); - /* If we're not moving there is nothing more to do */ if (!moving){ - /* - * Reset PIDs once, to prevent startup spikes, - * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/ - * PrevInput is considered a good proxy to detect - * whether reset has already happened - */ if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID(); return; } /* Compute PID update for each motor */ - doPID(&rightPID); doPID(&leftPID); - - /* Set the motor speeds accordingly */ + doPID(&rightPID); setMotorSpeeds(leftPID.output, rightPID.output); } diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h index 8d35ab1..6ff3fc2 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h @@ -1,18 +1,8 @@ -/* ************************************************************* - Encoder driver function definitions - by James Nugen - ************************************************************ */ - - -#ifdef ARDUINO_ENC_COUNTER - //below can be changed, but should be PORTD pins; - //otherwise additional changes in the code are required #define LEFT_ENC_PIN_A PD2 //pin 2 #define LEFT_ENC_PIN_B PD3 //pin 3 - //below can be changed, but should be PORTC pins #define RIGHT_ENC_PIN_A PC4 //pin A4 #define RIGHT_ENC_PIN_B PC5 //pin A5 -#endif long readEncoder(int i); void resetEncoder(int i); diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino index ef2b8b1..c53e998 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino @@ -1,38 +1,7 @@ -/* ************************************************************* - Encoder definitions - - Add an "#ifdef" block to this file to include support for - a particular encoder board or library. Then add the appropriate - #define near the top of the main ROSArduinoBridge.ino file. - - ************************************************************ */ - -#ifdef USE_BASE - -#ifdef ROBOGAIA - /* The Robogaia Mega Encoder shield */ - #include "MegaEncoderCounter.h" - - /* Create the encoder shield object */ - MegaEncoderCounter encoders = MegaEncoderCounter(4); // Initializes the Mega Encoder Counter in the 4X Count mode - - /* Wrap the encoder reading function */ - long readEncoder(int i) { - if (i == LEFT) return encoders.YAxisGetCount(); - else return encoders.XAxisGetCount(); - } - - /* Wrap the encoder reset function */ - void resetEncoder(int i) { - if (i == LEFT) return encoders.YAxisReset(); - else return encoders.XAxisReset(); - } -#elif defined(ARDUINO_ENC_COUNTER) volatile long left_enc_pos = 0L; volatile long right_enc_pos = 0L; static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0}; //encoder lookup table - /* Interrupt routine for LEFT encoder, taking care of actual counting */ ISR (PCINT2_vect){ static uint8_t enc_last=0; @@ -42,7 +11,6 @@ left_enc_pos += ENC_STATES[(enc_last & 0x0f)]; } - /* Interrupt routine for RIGHT encoder, taking care of actual counting */ ISR (PCINT1_vect){ static uint8_t enc_last=0; @@ -52,13 +20,11 @@ right_enc_pos += ENC_STATES[(enc_last & 0x0f)]; } - /* Wrap the encoder reading function */ long readEncoder(int i) { if (i == LEFT) return left_enc_pos; else return right_enc_pos; } - /* Wrap the encoder reset function */ void resetEncoder(int i) { if (i == LEFT){ left_enc_pos=0L; @@ -68,15 +34,10 @@ return; } } -#else - #error A encoder driver must be selected! -#endif -/* Wrap the encoder reset function */ void resetEncoders() { resetEncoder(LEFT); resetEncoder(RIGHT); } -#endif diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h index 9bf3f16..326da1d 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h @@ -1,15 +1,9 @@ -/*************************************************************** - Motor driver function definitions - by James Nugen - *************************************************************/ - -#ifdef L298_MOTOR_DRIVER #define RIGHT_MOTOR_BACKWARD 5 #define LEFT_MOTOR_BACKWARD 6 #define RIGHT_MOTOR_FORWARD 9 #define LEFT_MOTOR_FORWARD 10 #define RIGHT_MOTOR_ENABLE 12 #define LEFT_MOTOR_ENABLE 13 -#endif void initMotorController(); void setMotorSpeed(int i, int spd); diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino index facb8a8..9024795 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino @@ -1,61 +1,3 @@ -/*************************************************************** - Motor driver definitions - - Add a "#elif defined" block to this file to include support - for a particular motor driver. Then add the appropriate - #define near the top of the main ROSArduinoBridge.ino file. - - *************************************************************/ - -#ifdef USE_BASE - -#ifdef POLOLU_VNH5019 - /* Include the Pololu library */ - #include "DualVNH5019MotorShield.h" - - /* Create the motor driver object */ - DualVNH5019MotorShield drive; - - /* Wrap the motor driver initialization */ - void initMotorController() { - drive.init(); - } - - /* Wrap the drive motor set speed function */ - void setMotorSpeed(int i, int spd) { - if (i == LEFT) drive.setM1Speed(spd); - else drive.setM2Speed(spd); - } - - // A convenience function for setting both motor speeds - void setMotorSpeeds(int leftSpeed, int rightSpeed) { - setMotorSpeed(LEFT, leftSpeed); - setMotorSpeed(RIGHT, rightSpeed); - } -#elif defined POLOLU_MC33926 - /* Include the Pololu library */ - #include "DualMC33926MotorShield.h" - - /* Create the motor driver object */ - DualMC33926MotorShield drive; - - /* Wrap the motor driver initialization */ - void initMotorController() { - drive.init(); - } - - /* Wrap the drive motor set speed function */ - void setMotorSpeed(int i, int spd) { - if (i == LEFT) drive.setM1Speed(spd); - else drive.setM2Speed(spd); - } - - // A convenience function for setting both motor speeds - void setMotorSpeeds(int leftSpeed, int rightSpeed) { - setMotorSpeed(LEFT, leftSpeed); - setMotorSpeed(RIGHT, rightSpeed); - } -#elif defined L298_MOTOR_DRIVER void initMotorController() { digitalWrite(RIGHT_MOTOR_ENABLE, HIGH); digitalWrite(LEFT_MOTOR_ENABLE, HIGH); @@ -73,21 +15,18 @@ spd = 255; if (i == LEFT) { - if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); } - else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); } - } - else /*if (i == RIGHT) //no need for condition*/ { - if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); } + if(spd == 0) { digitalWrite(LEFT_MOTOR_FORWARD, 1); digitalWrite(LEFT_MOTOR_BACKWARD, 1); } + else if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); } else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); } } + else if (i == RIGHT){ + if(spd == 0) { digitalWrite(RIGHT_MOTOR_FORWARD, 1); digitalWrite(RIGHT_MOTOR_BACKWARD, 1); } + else if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); } + else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); } + } } void setMotorSpeeds(int leftSpeed, int rightSpeed) { setMotorSpeed(LEFT, leftSpeed); setMotorSpeed(RIGHT, rightSpeed); } -#else - #error A motor driver must be selected! -#endif - -#endif diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h deleted file mode 100644 index 75caeb7..0000000 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h +++ /dev/null @@ -1,34 +0,0 @@ -/* Functions for various sensor types */ - -float microsecondsToCm(long microseconds) -{ - // The speed of sound is 340 m/s or 29 microseconds per cm. - // The ping travels out and back, so to find the distance of the - // object we take half of the distance travelled. - return microseconds / 29 / 2; -} - -long Ping(int pin) { - long duration, range; - - // The PING))) is triggered by a HIGH pulse of 2 or more microseconds. - // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: - pinMode(pin, OUTPUT); - digitalWrite(pin, LOW); - delayMicroseconds(2); - digitalWrite(pin, HIGH); - delayMicroseconds(5); - digitalWrite(pin, LOW); - - // The same pin is used to read the signal from the PING))): a HIGH - // pulse whose duration is the time (in microseconds) from the sending - // of the ping to the reception of its echo off of an object. - pinMode(pin, INPUT); - duration = pulseIn(pin, HIGH); - - // convert the time into meters - range = microsecondsToCm(duration); - - return(range); -} - diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h deleted file mode 100644 index d2b6d21..0000000 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef SERVOS_H -#define SERVOS_H - - -#define N_SERVOS 2 - -// This delay in milliseconds determines the pause -// between each one degree step the servo travels. Increasing -// this number will make the servo sweep more slowly. -// Decreasing this number will make the servo sweep more quickly. -// Zero is the default number and will make the servos spin at -// full speed. 150 ms makes them spin very slowly. -int stepDelay [N_SERVOS] = { 0, 0 }; // ms - -// Pins -byte servoPins [N_SERVOS] = { 3, 4 }; - -// Initial Position -byte servoInitPosition [N_SERVOS] = { 90, 90 }; // [0, 180] degrees - - -class SweepServo -{ - public: - SweepServo(); - void initServo( - int servoPin, - int stepDelayMs, - int initPosition); - void doSweep(); - void setTargetPosition(int position); - Servo getServo(); - - private: - Servo servo; - int stepDelayMs; - int currentPositionDegrees; - int targetPositionDegrees; - long lastSweepCommand; -}; - -SweepServo servos [N_SERVOS]; - -#endif diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino deleted file mode 100644 index 5ba344c..0000000 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino +++ /dev/null @@ -1,78 +0,0 @@ -/*************************************************************** - Servo Sweep - by Nathaniel Gallinger - - Sweep servos one degree step at a time with a user defined - delay in between steps. Supports changing direction - mid-sweep. Important for applications such as robotic arms - where the stock servo speed is too fast for the strength - of your system. - - *************************************************************/ - -#ifdef USE_SERVOS - - -// Constructor -SweepServo::SweepServo() -{ - this->currentPositionDegrees = 0; - this->targetPositionDegrees = 0; - this->lastSweepCommand = 0; -} - - -// Init -void SweepServo::initServo( - int servoPin, - int stepDelayMs, - int initPosition) -{ - this->servo.attach(servoPin); - this->stepDelayMs = stepDelayMs; - this->currentPositionDegrees = initPosition; - this->targetPositionDegrees = initPosition; - this->lastSweepCommand = millis(); -} - - -// Perform Sweep -void SweepServo::doSweep() -{ - - // Get ellapsed time - int delta = millis() - this->lastSweepCommand; - - // Check if time for a step - if (delta > this->stepDelayMs) { - // Check step direction - if (this->targetPositionDegrees > this->currentPositionDegrees) { - this->currentPositionDegrees++; - this->servo.write(this->currentPositionDegrees); - } - else if (this->targetPositionDegrees < this->currentPositionDegrees) { - this->currentPositionDegrees--; - this->servo.write(this->currentPositionDegrees); - } - // if target == current position, do nothing - - // reset timer - this->lastSweepCommand = millis(); - } -} - - -// Set a new target position -void SweepServo::setTargetPosition(int position) -{ - this->targetPositionDegrees = position; -} - - -// Accessor for servo object -Servo SweepServo::getServo() -{ - return this->servo; -} - - -#endif diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml index 9acbe3d..f66b63f 100644 --- a/ros_arduino_python/config/arduino_params.yaml +++ b/ros_arduino_python/config/arduino_params.yaml @@ -24,10 +24,15 @@ base_frame: base_link #motors_reversed: True # === PID parameters -#Kp: 10 -#Kd: 12 -#Ki: 0 -#Ko: 50 +#lKp: 10 +#lKd: 12 +#lKi: 0 +#lKo: 50 +#rKp: 10 +#rKd: 12 +#rKi: 0 +#rKo: 50 + #accel_limit: 1.0 # === Sensor definitions. Examples only - edit for your robot. diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py index 68f0a6b..4452f6c 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -248,12 +248,14 @@ class Arduino: self.mutex.release() return ack == 'OK' - def update_pid(self, Kp, Kd, Ki, Ko): + def update_pid(self, lKp, lKd, lKi, lKo, rKp, rKd, rKi, rKo): ''' Set the PID parameters on the Arduino ''' print "Updating PID parameters" - cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko) + cmd = 'L ' + str(lKp) + ':' + str(lKd) + ':' + str(lKi) + ':' + str(lKo) self.execute_ack(cmd) + cmd = 'R ' + str(rKp) + ':' + str(rKd) + ':' + str(rKi) + ':' + str(rKo) + self.execute_ack(cmd) def get_baud(self): ''' Get the current baud rate on the serial port. diff --git a/ros_arduino_python/src/ros_arduino_python/base_controller.py b/ros_arduino_python/src/ros_arduino_python/base_controller.py index 4c5092e..859c1bb 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -44,11 +44,16 @@ class BaseController: pid_params['wheel_track'] = rospy.get_param("~wheel_track", "") pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0) - pid_params['Kp'] = rospy.get_param("~Kp", 20) - pid_params['Kd'] = rospy.get_param("~Kd", 12) - pid_params['Ki'] = rospy.get_param("~Ki", 0) - pid_params['Ko'] = rospy.get_param("~Ko", 50) + pid_params['lKp'] = rospy.get_param("~lKp", 20) + pid_params['lKd'] = rospy.get_param("~lKd", 12) + pid_params['lKi'] = rospy.get_param("~lKi", 0) + pid_params['lKo'] = rospy.get_param("~lKo", 50) + pid_params['rKp'] = rospy.get_param("~rKp", 20) + pid_params['rKd'] = rospy.get_param("~rKd", 12) + pid_params['rKi'] = rospy.get_param("~rKi", 0) + pid_params['rKo'] = rospy.get_param("~rKo", 50) + self.accel_limit = rospy.get_param('~accel_limit', 0.1) self.motors_reversed = rospy.get_param("~motors_reversed", False) @@ -90,7 +95,7 @@ class BaseController: # Set up the odometry broadcaster self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5) self.odomBroadcaster = TransformBroadcaster() - + self.cameraBroadcaster = TransformBroadcaster() rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev") rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame") @@ -110,12 +115,17 @@ class BaseController: self.encoder_resolution = pid_params['encoder_resolution'] self.gear_reduction = pid_params['gear_reduction'] - self.Kp = pid_params['Kp'] - self.Kd = pid_params['Kd'] - self.Ki = pid_params['Ki'] - self.Ko = pid_params['Ko'] + self.lKp = pid_params['lKp'] + self.lKd = pid_params['lKd'] + self.lKi = pid_params['lKi'] + self.lKo = pid_params['lKo'] - self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko) + self.rKp = pid_params['rKp'] + self.rKd = pid_params['rKd'] + self.rKi = pid_params['rKi'] + self.rKo = pid_params['rKo'] + + self.arduino.update_pid(self.lKp, self.lKd, self.lKi, self.lKo,self.rKp, self.rKd, self.rKi, self.rKo) def poll(self): now = rospy.Time.now() @@ -171,7 +181,11 @@ class BaseController: self.base_frame, "odom" ) - + self.odomBroadcaster.sendTransform((self.x - 0.15, self.y - 0.055, 0.225), + (quaternion.x, quaternion.y, quaternion.z, quaternion.w), + rospy.Time.now(), + "depth_camera_frame", + self.base_frame) odom = Odometry() odom.header.frame_id = "odom" odom.child_frame_id = self.base_frame