mbed Rover
The mbed Rover is the result of combining the Stinger Robot Kit, the QEI, PID, and IMU libraries along with a ITG3200 gyroscope, ADXL345 accelerometer, and a pair of Polulu MD10B breakout boards driven by the Motor library.
Simple Rover¶
The simplest implementation involves driving the motors forward a set number of pulses as measured by the quadrature encoders. This involves no speed feedback from the motors and causes the rover to drift in different directions in a non-deterministic manner. If there was a bias to the direction of drift it would also be difficult to correct it with simply changing the speed of one wheel as we would most likely want to dynamically change the speed of the wheel throughout any movement (e.g. increase PWM signal at the beginning to overcome static friction, then decrease to a lower level).
Note: We've swapped inA, inB, chanA and chanB for the motors to ensure that both motors respond to the same direction and give the same increasing or decreasing pulse count; if we were to leave them the same when we drove the robot "forward", one encoder would count up while the other one would count down since they would technically be rotating in opposite direction. We remove this ambiguity by swapping the channels for one motor.
Import program
00001 /** 00002 * Drive a robot forwards or backwards by supplying a PWM signal to 00003 * H-bridges connected to the motors. 00004 */ 00005 00006 #include "Motor.h" 00007 #include "QEI.h" 00008 00009 #define PULSES_PER_REVOLUTION 624 00010 00011 Motor leftMotor(p21, p20, p19); //pwm, inB, inA 00012 Motor rightMotor(p22, p17, p18); //pwm, inA, inB 00013 QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr 00014 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr 00015 00016 int main() { 00017 00018 leftMotor.period(0.00005); //Set motor PWM periods to 20KHz. 00019 rightMotor.period(0.00005); 00020 00021 int leftPulses = 0; //How far the left wheel has travelled. 00022 int rightPulses = 0; //How far the right wheel has travelled. 00023 int distance = 6000; //Number of pulses to travel. 00024 00025 wait(5); //Wait a few seconds before we start moving. 00026 00027 leftMotor.speed(0.4); 00028 rightMotor.speed(0.4); 00029 00030 while((leftPulses < distance) || (rightPulses < distance)){ 00031 00032 leftPulses = leftQei.getPulses(); 00033 rightPulses = rightQei.getPulses(); 00034 00035 } 00036 00037 leftMotor.brake(); 00038 rightMotor.brake(); 00039 00040 }
PID Rover¶
A more advanced implementation uses a PID controller to try and keep the speeds of the two motors the same. The PID tuning constants for the specific motors were calculated using the method outlined in the PID cookbook page. As this method dynamically controls the speed of the motors, we can change the speed set point of one motor if we find a consistent bias in any drift along a straight line section.
Import program
00001 /** 00002 * Drive a robot forwards or backwards by using a PID controller to vary 00003 * the PWM signal to H-bridges connected to the motors to attempt to maintain 00004 * a constant velocity. 00005 */ 00006 00007 #include "Motor.h" 00008 #include "QEI.h" 00009 #include "PID.h" 00010 00011 Motor leftMotor(p21, p20, p19); //pwm, inB, inA 00012 Motor rightMotor(p22, p17, p18); //pwm, inA, inB 00013 QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr 00014 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr 00015 //Tuning parameters calculated from step tests; 00016 //see http://mbed.org/cookbook/PID for examples. 00017 PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td 00018 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td 00019 00020 int main() { 00021 00022 leftMotor.period(0.00005); //Set motor PWM periods to 20KHz. 00023 rightMotor.period(0.00005); 00024 00025 //Input and output limits have been determined 00026 //empirically with the specific motors being used. 00027 //Change appropriately for different motors. 00028 //Input units: counts per second. 00029 //Output units: PwmOut duty cycle as %. 00030 //Default limits are for moving forward. 00031 leftPid.setInputLimits(0, 3000); 00032 leftPid.setOutputLimits(0.0, 0.9); 00033 leftPid.setMode(AUTO_MODE); 00034 rightPid.setInputLimits(0, 3200); 00035 rightPid.setOutputLimits(0.0, 0.9); 00036 rightPid.setMode(AUTO_MODE); 00037 00038 00039 int leftPulses = 0; //How far the left wheel has travelled. 00040 int leftPrevPulses = 0; //The previous reading of how far the left wheel 00041 //has travelled. 00042 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per 00043 //second. 00044 int rightPulses = 0; //How far the right wheel has travelled. 00045 int rightPrevPulses = 0; //The previous reading of how far the right wheel 00046 //has travelled. 00047 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per 00048 //second. 00049 int distance = 6000; //Number of pulses to travel. 00050 00051 wait(5); //Wait a few seconds before we start moving. 00052 00053 //Velocity to mantain in pulses per second. 00054 leftPid.setSetPoint(1000); 00055 rightPid.setSetPoint(975); 00056 00057 while ((leftPulses < distance) || (rightPulses < distance)) { 00058 00059 //Get the current pulse count and subtract the previous one to 00060 //calculate the current velocity in counts per second. 00061 leftPulses = leftQei.getPulses(); 00062 leftVelocity = (leftPulses - leftPrevPulses) / 0.01; 00063 leftPrevPulses = leftPulses; 00064 //Use the absolute value of velocity as the PID controller works 00065 //in the % (unsigned) domain and will get confused with -ve values. 00066 leftPid.setProcessValue(fabs(leftVelocity)); 00067 leftMotor.speed(leftPid.compute()); 00068 00069 rightPulses = rightQei.getPulses(); 00070 rightVelocity = (rightPulses - rightPrevPulses) / 0.01; 00071 rightPrevPulses = rightPulses; 00072 rightPid.setProcessValue(fabs(rightVelocity)); 00073 rightMotor.speed(rightPid.compute()); 00074 00075 wait(0.01); 00076 00077 } 00078 00079 leftMotor.brake(); 00080 rightMotor.brake(); 00081 00082 }
IMU Rover¶
The final implementation incorporates an inertial measurement unit consisting of an ITG3200 gyroscope and ADXL345 accelerometer. It also follows a set of movement commands contained in a .txt file on the mbed filesystem which are things like "move forward 0.75" and "turn right 90" which correspond to moving forward in a straight line for 0.75m and turning clockwise 90 degrees.
It keeps track of the heading at the start of a straight line section and takes another measurement at the end so that it can correct any drift on a subsequent turn. This is because the rover was primarily designed to complete a dead reckoning test of driving round a square and returning to its starting point and so could reduce errors by correcting the turn angle on each straight line segment. This may not be appropriate for all applications.
Note: Some calibration is required for the IMU. At the data rate I was using for the IMU and the surface the rover was on, the turn commands actually required a parameter of 86 for the degrees as opposed to 90, when the rover wanted to make a 90 degree turn. Motor speed and angle to turn parameters may need to be calibrated for a different set up.
Import program
00001 /** 00002 * Includes. 00003 */ 00004 #include "Motor.h" 00005 #include "QEI.h" 00006 #include "PID.h" 00007 #include "IMU.h" 00008 00009 /** 00010 * Defines. 00011 */ 00012 #define BUFFER_SIZE 1024 //Used for data logging arrays 00013 #define RATE 0.01 //PID loop timing 00014 00015 //PID tuning constants. 00016 #define L_KC 0.4312 //Forward left motor Kc 00017 #define L_TI 0.1 //Forward left motor Ti 00018 #define L_TD 0.0 //Forward left motor Td 00019 #define R_KC 0.4620 //Forward right motor Kc 00020 #define R_TI 0.1 //Forward right motor Ti 00021 #define R_TD 0.0 //Forward right motor Td 00022 00023 //PID input/output limits. 00024 #define L_INPUT_MIN 0 //Forward left motor minimum input 00025 #define L_INPUT_MAX 3000 //Forward left motor maximum input 00026 #define L_OUTPUT_MIN 0.0 //Forward left motor minimum output 00027 #define L_OUTPUT_MAX 0.9 //Forward left motor maximum output 00028 00029 #define R_INPUT_MIN 0 //Forward right motor input minimum 00030 #define R_INPUT_MAX 3200 //Forward right motor input maximum 00031 #define R_OUTPUT_MIN 0.0 //Forward right motor output minimum 00032 #define R_OUTPUT_MAX 0.9 //Forward right motor output maximum 00033 00034 //Physical dimensions. 00035 #define PULSES_PER_REV 624 00036 #define WHEEL_DIAMETER 58.928 //mm 00037 #define ROTATION_DISTANCE 220.0 //mm 00038 #define REVS_PER_ROTATION (ROTATION_DISTANCE / WHEEL_DIAMETER) 00039 #define PULSES_PER_ROTATION (REVS_PER_ROTATION * PULSES_PER_REV) 00040 #define PULSES_PER_MM (PULSES_PER_REV / WHEEL_DIAMETER) 00041 #define DISTANCE_PER_PULSE (WHEEL_DIAMETER / PULSES_PER_REV) 00042 #define ENCODING 2 //Use X2 encoding 00043 #define WHEEL_DISTANCE (ROTATION_DISTANCE / DISTANCE_PER_PULSE) 00044 00045 /** 00046 * Function Prototypes 00047 */ 00048 void initializeMotors(void); 00049 void initializePid(void); 00050 //cmd = "move", "turn" 00051 //direction = "forward", "backward", "left", "right" 00052 //length = distance in metres or angle in degrees 00053 void processCommand(char* cmd, char* direction, float length); 00054 00055 /** 00056 * Globals 00057 */ 00058 00059 //Debug. 00060 Serial pc(USBTX, USBRX); 00061 00062 //Motor control. 00063 Motor leftMotor(p21, p20, p19); //pwm, inB, inA 00064 Motor rightMotor(p22, p17, p18); //pwm, inA, inB 00065 QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr 00066 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr 00067 PID leftPid(L_KC, L_TI, L_TD, RATE); //Kc, Ti, Td 00068 PID rightPid(R_KC, R_TI, R_TD, RATE); //Kc, Ti, Td 00069 //IMU and peripherals run at a different rate to the main PID loop. 00070 IMU imu(IMU_RATE, GYRO_MEAS_ERROR, ACCELEROMETER_RATE, GYROSCOPE_RATE); 00071 00072 //Filesystem. 00073 LocalFileSystem local("local"); 00074 00075 //Left motor working variables. 00076 int leftPulses = 0; 00077 int leftPrevPulses = 0; 00078 float leftVelocity = 0.0; 00079 float leftVelocityBuffer[BUFFER_SIZE]; 00080 00081 //Right motor working variables. 00082 int rightPulses = 0; 00083 int rightPrevPulses = 0; 00084 float rightVelocity = 0.0; 00085 float rightVelocityBuffer[BUFFER_SIZE]; 00086 00087 //General working variables. 00088 float heading = 0.0; 00089 float prevHeading = 0.0; 00090 float degreesTurned = 0.0; 00091 float positionSetPoint = 0.0; 00092 float headingSetPoint = 0.0; 00093 00094 //Store the initial and end heading during a straight line section 00095 //in order to be able to correct turns. 00096 float startHeading = 0.0; 00097 float endHeading = 0.0; 00098 00099 //Command Parser. 00100 char cmd0[16]; //{"move", "turn"} 00101 char cmd1[16]; //{{"forward", "backward"}, {"left", "right"}} 00102 float cmd2 = 0; //{distance in METRES, angle in DEGREES} 00103 00104 void initializeMotors(void) { 00105 00106 //Set motor PWM periods to 20KHz. 00107 leftMotor.period(0.00005); 00108 leftMotor.speed(0.0); 00109 00110 rightMotor.period(0.00005); 00111 rightMotor.speed(0.0); 00112 00113 } 00114 00115 void initializePid(void) { 00116 00117 //Input and output limits have been determined 00118 //empirically with the specific motors being used. 00119 //Change appropriately for different motors. 00120 //Input units: counts per second. 00121 //Output units: PwmOut duty cycle as %. 00122 //Default limits are for moving forward. 00123 leftPid.setInputLimits(L_INPUT_MIN, L_INPUT_MAX); 00124 leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX); 00125 leftPid.setMode(AUTO_MODE); 00126 rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX); 00127 rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX); 00128 rightPid.setMode(AUTO_MODE); 00129 00130 } 00131 00132 void processCommand(char* cmd, char* direction, float length) { 00133 00134 //move command. 00135 if (strcmp(cmd, "move") == 0) { 00136 00137 int i = 0; //Data log array index. 00138 00139 //The PID controller works in the % (unsigned) domain, so we'll 00140 //need to multiply the output by -1 if our motors need 00141 //to go "backwards". 00142 int leftDirection = 1; 00143 int rightDirection = 1; 00144 00145 //Convert from metres into millimetres. 00146 length *= 1000; 00147 //Work out how many pulses are required to go that many millimetres. 00148 length *= PULSES_PER_MM; 00149 //Make sure we scale the number of pulses according to our encoding method. 00150 length /= ENCODING; 00151 00152 positionSetPoint = length; 00153 00154 if (strcmp(direction, "forward") == 0) { 00155 //Leave left and rightDirection as +ve. 00156 } else if (strcmp(cmd1, "backward") == 0) { 00157 //Change left and rightDirection to -ve. 00158 leftDirection = -1; 00159 rightDirection = -1; 00160 } 00161 00162 startHeading = imu.getYaw(); 00163 00164 //With left set point == right set point, the rover veers off to the 00165 //right - by slowing the right motor down slightly it goes relatively 00166 //straight. 00167 leftPid.setSetPoint(1000); 00168 rightPid.setSetPoint(975); 00169 00170 //Keep going until we've moved the correct distance defined by the 00171 //position set point. Take the absolute value of the pulses as we might 00172 //be moving backwards. 00173 while ((abs(leftPulses) < positionSetPoint) && 00174 (abs(rightPulses) < positionSetPoint)) { 00175 00176 //Get the current pulse count and subtract the previous one to 00177 //calculate the current velocity in counts per second. 00178 leftPulses = leftQei.getPulses(); 00179 leftVelocity = (leftPulses - leftPrevPulses) / RATE; 00180 leftPrevPulses = leftPulses; 00181 //Use the absolute value of velocity as the PID controller works 00182 //in the % (unsigned) domain and will get confused with -ve values. 00183 leftPid.setProcessValue(fabs(leftVelocity)); 00184 leftMotor.speed(leftPid.compute() * leftDirection); 00185 00186 rightPulses = rightQei.getPulses(); 00187 rightVelocity = (rightPulses - rightPrevPulses) / RATE; 00188 rightPrevPulses = rightPulses; 00189 rightPid.setProcessValue(fabs(rightVelocity)); 00190 rightMotor.speed(rightPid.compute() * rightDirection); 00191 00192 i++; 00193 00194 wait(RATE); 00195 00196 } 00197 00198 leftMotor.brake(); 00199 rightMotor.brake(); 00200 00201 endHeading = imu.getYaw(); 00202 00203 } 00204 //turn command. 00205 else if (strcmp(cmd0, "turn") == 0) { 00206 00207 //The PID controller works in the % (unsigned) domain, so we'll 00208 //need to multiply the output by -1 for the motor which needs 00209 //to go "backwards". 00210 int leftDirection = 1; 00211 int rightDirection = 1; 00212 headingSetPoint = length + (endHeading - startHeading); 00213 00214 //In case the rover tries to [pointlessly] turn >360 degrees. 00215 if (headingSetPoint > 359.8) { 00216 00217 headingSetPoint -= 359.8; 00218 00219 } 00220 00221 //Set up the right previous heading for the initial degreesTurned 00222 //calculation. 00223 prevHeading = fabs(imu.getYaw()); 00224 00225 if (strcmp(cmd1, "left") == 0) { 00226 00227 //When turning left, the left motor needs to go backwards 00228 //while the right motor goes forwards. 00229 leftDirection = -1; 00230 00231 } else if (strcmp(cmd1, "right") == 0) { 00232 00233 //When turning right, the right motor needs to go backwards 00234 //while the left motor goes forwards. 00235 rightDirection = -1; 00236 00237 } 00238 00239 //Turn slowly to ensure we don't overshoot the angle by missing 00240 //the relatively slow readings [in comparison to the PID loop speed] 00241 //from the IMU. 00242 leftPid.setSetPoint(500); 00243 rightPid.setSetPoint(500); 00244 00245 //Keep turning until we've moved through the correct angle as defined 00246 //by the heading set point. 00247 while (degreesTurned < headingSetPoint) { 00248 00249 //Get the new heading and subtract the previous heading to 00250 //determine how many more degrees we've moved through. 00251 //As we're only interested in the relative angle (as opposed to 00252 //absolute) we'll take the absolute value of the heading to avoid 00253 //any nastiness when trying to calculate angles as they jump from 00254 //179.9 to -179.9 degrees. 00255 heading = fabs(imu.getYaw()); 00256 degreesTurned += fabs(heading - prevHeading); 00257 prevHeading = heading; 00258 00259 //Get the current pulse count and subtract the previous one to 00260 //calculate the current velocity in counts per second. 00261 leftPulses = leftQei.getPulses(); 00262 leftVelocity = (leftPulses - leftPrevPulses) / RATE; 00263 leftPrevPulses = leftPulses; 00264 //Use the absolute value of velocity as the PID controller works 00265 //in the % (unsigned) domain and will get confused with -ve values. 00266 leftPid.setProcessValue(fabs(leftVelocity)); 00267 leftMotor.speed(leftPid.compute() * leftDirection); 00268 00269 rightPulses = rightQei.getPulses(); 00270 rightVelocity = (rightPulses - rightPrevPulses) / RATE; 00271 rightPrevPulses = rightPulses; 00272 rightPid.setProcessValue(fabs(rightVelocity)); 00273 rightMotor.speed(rightPid.compute() * rightDirection); 00274 00275 wait(RATE); 00276 00277 } 00278 00279 leftMotor.brake(); 00280 rightMotor.brake(); 00281 00282 } 00283 00284 //Reset working variables. 00285 leftQei.reset(); 00286 rightQei.reset(); 00287 00288 leftPulses = 0; 00289 leftPrevPulses = 0; 00290 leftVelocity = 0.0; 00291 leftMotor.speed(0.0); 00292 00293 rightPulses = 0; 00294 rightPrevPulses = 0; 00295 rightVelocity = 0.0; 00296 rightMotor.speed(0.0); 00297 00298 leftPid.setSetPoint(0.0); 00299 leftPid.setProcessValue(0.0); 00300 rightPid.setSetPoint(0.0); 00301 rightPid.setProcessValue(0.0); 00302 00303 positionSetPoint = 0.0; 00304 headingSetPoint = 0.0; 00305 heading = 0.0; 00306 prevHeading = 0.0; 00307 degreesTurned = 0.0; 00308 00309 } 00310 00311 int main() { 00312 00313 pc.printf("Starting mbed rover test...\n"); 00314 00315 initializeMotors(); 00316 initializePid(); 00317 00318 //Some delay before we start moving. 00319 wait(5); 00320 00321 //Open the command script. 00322 FILE* commands = fopen("/local/commands.txt", "r"); 00323 00324 //Check if we were successful in opening the command script. 00325 if (commands == NULL) { 00326 pc.printf("Could not open command file...\n"); 00327 exit(EXIT_FAILURE); 00328 } else { 00329 pc.printf("Succesfully opened command file!\n"); 00330 } 00331 00332 //While there's another line to read from the command script. 00333 while (fscanf(commands, "%s%s%f", cmd0, cmd1, &cmd2) >= 0) { 00334 00335 pc.printf("%s %s %f\n", cmd0, cmd1, cmd2); 00336 00337 processCommand(cmd0, cmd1, cmd2); 00338 00339 wait(1); 00340 00341 } 00342 00343 fclose(commands); 00344 00345 }
Future Work¶
While attempting to run the motors at the same speed using a PID controller provided better results than simply running them in an open loop configuration, they could still be improved. The obvious next step would be to introduce another PID loop which changed the motor speed set points depending on how the rover was drifting from its initial heading reading. If the heading angle started to drift to the right, the left motor speed would be reduced, and vice versa for left drift. The IMU is quite accurate over short time periods and distance, and frequent re-calibration could be performed as it only takes a fraction of a second.
The addition of a wireless communications module would also allow a more natural and entertaining interface, allow commands to be given on the fly instead of having to a follow a static command script.