## 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 programSimpleRover - main.cpp

```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 programPIDRover - main.cpp

```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 programIMURover - main.cpp

```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.
00090 float degreesTurned = 0.0;
00091 float positionSetPoint = 0.0;
00093
00094 //Store the initial and end heading during a straight line section
00095 //in order to be able to correct turns.
00098
00099 //Command Parser.
00100 char  cmd0; //{"move", "turn"}
00101 char  cmd1; //{{"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
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
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;
00213
00214         //In case the rover tries to [pointlessly] turn >360 degrees.
00215         if (headingSetPoint > 359.8) {
00216
00218
00219         }
00220
00221         //Set up the right previous heading for the initial degreesTurned
00222         //calculation.
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
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.
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;
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.