mbed Rover

http://mbed.org/media/uploads/aberk/_scaled_mbedrover.jpg

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.