Aaron Berk / Mbed 2 deprecated IMURover

An advanced robot that uses PID control on the motor speed, and an IMU for making accurate turns.

Dependencies:   mbed Motor ITG3200 QEI ADXL345 IMUfilter PID

main.cpp@0:7440a03255a7, 2010-09-10 (annotated)

Committer:
aberk
Date:
Fri Sep 10 14:03:00 2010 +0000
Revision:
0:7440a03255a7
Version 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:7440a03255a7 1 /**
aberk 0:7440a03255a7 2 * Includes.
aberk 0:7440a03255a7 3 */
aberk 0:7440a03255a7 4 #include "Motor.h"
aberk 0:7440a03255a7 5 #include "QEI.h"
aberk 0:7440a03255a7 6 #include "PID.h"
aberk 0:7440a03255a7 7 #include "IMU.h"
aberk 0:7440a03255a7 8
aberk 0:7440a03255a7 9 /**
aberk 0:7440a03255a7 10 * Defines.
aberk 0:7440a03255a7 11 */
aberk 0:7440a03255a7 12 #define BUFFER_SIZE 1024 //Used for data logging arrays
aberk 0:7440a03255a7 13 #define RATE 0.01 //PID loop timing
aberk 0:7440a03255a7 14
aberk 0:7440a03255a7 15 //PID tuning constants.
aberk 0:7440a03255a7 16 #define L_KC 0.4312 //Forward left motor Kc
aberk 0:7440a03255a7 17 #define L_TI 0.1 //Forward left motor Ti
aberk 0:7440a03255a7 18 #define L_TD 0.0 //Forward left motor Td
aberk 0:7440a03255a7 19 #define R_KC 0.4620 //Forward right motor Kc
aberk 0:7440a03255a7 20 #define R_TI 0.1 //Forward right motor Ti
aberk 0:7440a03255a7 21 #define R_TD 0.0 //Forward right motor Td
aberk 0:7440a03255a7 22
aberk 0:7440a03255a7 23 //PID input/output limits.
aberk 0:7440a03255a7 24 #define L_INPUT_MIN 0 //Forward left motor minimum input
aberk 0:7440a03255a7 25 #define L_INPUT_MAX 3000 //Forward left motor maximum input
aberk 0:7440a03255a7 26 #define L_OUTPUT_MIN 0.0 //Forward left motor minimum output
aberk 0:7440a03255a7 27 #define L_OUTPUT_MAX 0.9 //Forward left motor maximum output
aberk 0:7440a03255a7 28
aberk 0:7440a03255a7 29 #define R_INPUT_MIN 0 //Forward right motor input minimum
aberk 0:7440a03255a7 30 #define R_INPUT_MAX 3200 //Forward right motor input maximum
aberk 0:7440a03255a7 31 #define R_OUTPUT_MIN 0.0 //Forward right motor output minimum
aberk 0:7440a03255a7 32 #define R_OUTPUT_MAX 0.9 //Forward right motor output maximum
aberk 0:7440a03255a7 33
aberk 0:7440a03255a7 34 //Physical dimensions.
aberk 0:7440a03255a7 35 #define PULSES_PER_REV 624
aberk 0:7440a03255a7 36 #define WHEEL_DIAMETER 58.928 //mm
aberk 0:7440a03255a7 37 #define ROTATION_DISTANCE 220.0 //mm
aberk 0:7440a03255a7 38 #define REVS_PER_ROTATION (ROTATION_DISTANCE / WHEEL_DIAMETER)
aberk 0:7440a03255a7 39 #define PULSES_PER_ROTATION (REVS_PER_ROTATION * PULSES_PER_REV)
aberk 0:7440a03255a7 40 #define PULSES_PER_MM (PULSES_PER_REV / WHEEL_DIAMETER)
aberk 0:7440a03255a7 41 #define DISTANCE_PER_PULSE (WHEEL_DIAMETER / PULSES_PER_REV)
aberk 0:7440a03255a7 42 #define ENCODING 2 //Use X2 encoding
aberk 0:7440a03255a7 43 #define WHEEL_DISTANCE (ROTATION_DISTANCE / DISTANCE_PER_PULSE)
aberk 0:7440a03255a7 44
aberk 0:7440a03255a7 45 /**
aberk 0:7440a03255a7 46 * Function Prototypes
aberk 0:7440a03255a7 47 */
aberk 0:7440a03255a7 48 void initializeMotors(void);
aberk 0:7440a03255a7 49 void initializePid(void);
aberk 0:7440a03255a7 50 //cmd = "move", "turn"
aberk 0:7440a03255a7 51 //direction = "forward", "backward", "left", "right"
aberk 0:7440a03255a7 52 //length = distance in metres or angle in degrees
aberk 0:7440a03255a7 53 void processCommand(char* cmd, char* direction, float length);
aberk 0:7440a03255a7 54
aberk 0:7440a03255a7 55 /**
aberk 0:7440a03255a7 56 * Globals
aberk 0:7440a03255a7 57 */
aberk 0:7440a03255a7 58
aberk 0:7440a03255a7 59 //Debug.
aberk 0:7440a03255a7 60 Serial pc(USBTX, USBRX);
aberk 0:7440a03255a7 61
aberk 0:7440a03255a7 62 //Motor control.
aberk 0:7440a03255a7 63 Motor leftMotor(p21, p20, p19); //pwm, inB, inA
aberk 0:7440a03255a7 64 Motor rightMotor(p22, p17, p18); //pwm, inA, inB
aberk 0:7440a03255a7 65 QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr
aberk 0:7440a03255a7 66 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
aberk 0:7440a03255a7 67 PID leftPid(L_KC, L_TI, L_TD, RATE); //Kc, Ti, Td
aberk 0:7440a03255a7 68 PID rightPid(R_KC, R_TI, R_TD, RATE); //Kc, Ti, Td
aberk 0:7440a03255a7 69 //IMU and peripherals run at a different rate to the main PID loop.
aberk 0:7440a03255a7 70 IMU imu(IMU_RATE, GYRO_MEAS_ERROR, ACCELEROMETER_RATE, GYROSCOPE_RATE);
aberk 0:7440a03255a7 71
aberk 0:7440a03255a7 72 //Filesystem.
aberk 0:7440a03255a7 73 LocalFileSystem local("local");
aberk 0:7440a03255a7 74
aberk 0:7440a03255a7 75 //Left motor working variables.
aberk 0:7440a03255a7 76 int leftPulses = 0;
aberk 0:7440a03255a7 77 int leftPrevPulses = 0;
aberk 0:7440a03255a7 78 float leftVelocity = 0.0;
aberk 0:7440a03255a7 79 float leftVelocityBuffer[BUFFER_SIZE];
aberk 0:7440a03255a7 80
aberk 0:7440a03255a7 81 //Right motor working variables.
aberk 0:7440a03255a7 82 int rightPulses = 0;
aberk 0:7440a03255a7 83 int rightPrevPulses = 0;
aberk 0:7440a03255a7 84 float rightVelocity = 0.0;
aberk 0:7440a03255a7 85 float rightVelocityBuffer[BUFFER_SIZE];
aberk 0:7440a03255a7 86
aberk 0:7440a03255a7 87 //General working variables.
aberk 0:7440a03255a7 88 float heading = 0.0;
aberk 0:7440a03255a7 89 float prevHeading = 0.0;
aberk 0:7440a03255a7 90 float degreesTurned = 0.0;
aberk 0:7440a03255a7 91 float positionSetPoint = 0.0;
aberk 0:7440a03255a7 92 float headingSetPoint = 0.0;
aberk 0:7440a03255a7 93
aberk 0:7440a03255a7 94 //Store the initial and end heading during a straight line section
aberk 0:7440a03255a7 95 //in order to be able to correct turns.
aberk 0:7440a03255a7 96 float startHeading = 0.0;
aberk 0:7440a03255a7 97 float endHeading = 0.0;
aberk 0:7440a03255a7 98
aberk 0:7440a03255a7 99 //Command Parser.
aberk 0:7440a03255a7 100 char cmd0[16]; //{"move", "turn"}
aberk 0:7440a03255a7 101 char cmd1[16]; //{{"forward", "backward"}, {"left", "right"}}
aberk 0:7440a03255a7 102 float cmd2 = 0; //{distance in METRES, angle in DEGREES}
aberk 0:7440a03255a7 103
aberk 0:7440a03255a7 104 void initializeMotors(void) {
aberk 0:7440a03255a7 105
aberk 0:7440a03255a7 106 //Set motor PWM periods to 20KHz.
aberk 0:7440a03255a7 107 leftMotor.period(0.00005);
aberk 0:7440a03255a7 108 leftMotor.speed(0.0);
aberk 0:7440a03255a7 109
aberk 0:7440a03255a7 110 rightMotor.period(0.00005);
aberk 0:7440a03255a7 111 rightMotor.speed(0.0);
aberk 0:7440a03255a7 112
aberk 0:7440a03255a7 113 }
aberk 0:7440a03255a7 114
aberk 0:7440a03255a7 115 void initializePid(void) {
aberk 0:7440a03255a7 116
aberk 0:7440a03255a7 117 //Input and output limits have been determined
aberk 0:7440a03255a7 118 //empirically with the specific motors being used.
aberk 0:7440a03255a7 119 //Change appropriately for different motors.
aberk 0:7440a03255a7 120 //Input units: counts per second.
aberk 0:7440a03255a7 121 //Output units: PwmOut duty cycle as %.
aberk 0:7440a03255a7 122 //Default limits are for moving forward.
aberk 0:7440a03255a7 123 leftPid.setInputLimits(L_INPUT_MIN, L_INPUT_MAX);
aberk 0:7440a03255a7 124 leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX);
aberk 0:7440a03255a7 125 leftPid.setMode(AUTO_MODE);
aberk 0:7440a03255a7 126 rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX);
aberk 0:7440a03255a7 127 rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX);
aberk 0:7440a03255a7 128 rightPid.setMode(AUTO_MODE);
aberk 0:7440a03255a7 129
aberk 0:7440a03255a7 130 }
aberk 0:7440a03255a7 131
aberk 0:7440a03255a7 132 void processCommand(char* cmd, char* direction, float length) {
aberk 0:7440a03255a7 133
aberk 0:7440a03255a7 134 //move command.
aberk 0:7440a03255a7 135 if (strcmp(cmd, "move") == 0) {
aberk 0:7440a03255a7 136
aberk 0:7440a03255a7 137 int i = 0; //Data log array index.
aberk 0:7440a03255a7 138
aberk 0:7440a03255a7 139 //The PID controller works in the % (unsigned) domain, so we'll
aberk 0:7440a03255a7 140 //need to multiply the output by -1 if our motors need
aberk 0:7440a03255a7 141 //to go "backwards".
aberk 0:7440a03255a7 142 int leftDirection = 1;
aberk 0:7440a03255a7 143 int rightDirection = 1;
aberk 0:7440a03255a7 144
aberk 0:7440a03255a7 145 //Convert from metres into millimetres.
aberk 0:7440a03255a7 146 length *= 1000;
aberk 0:7440a03255a7 147 //Work out how many pulses are required to go that many millimetres.
aberk 0:7440a03255a7 148 length *= PULSES_PER_MM;
aberk 0:7440a03255a7 149 //Make sure we scale the number of pulses according to our encoding method.
aberk 0:7440a03255a7 150 length /= ENCODING;
aberk 0:7440a03255a7 151
aberk 0:7440a03255a7 152 positionSetPoint = length;
aberk 0:7440a03255a7 153
aberk 0:7440a03255a7 154 if (strcmp(direction, "forward") == 0) {
aberk 0:7440a03255a7 155 //Leave left and rightDirection as +ve.
aberk 0:7440a03255a7 156 } else if (strcmp(cmd1, "backward") == 0) {
aberk 0:7440a03255a7 157 //Change left and rightDirection to -ve.
aberk 0:7440a03255a7 158 leftDirection = -1;
aberk 0:7440a03255a7 159 rightDirection = -1;
aberk 0:7440a03255a7 160 }
aberk 0:7440a03255a7 161
aberk 0:7440a03255a7 162 startHeading = imu.getYaw();
aberk 0:7440a03255a7 163
aberk 0:7440a03255a7 164 //With left set point == right set point, the rover veers off to the
aberk 0:7440a03255a7 165 //right - by slowing the right motor down slightly it goes relatively
aberk 0:7440a03255a7 166 //straight.
aberk 0:7440a03255a7 167 leftPid.setSetPoint(1000);
aberk 0:7440a03255a7 168 rightPid.setSetPoint(975);
aberk 0:7440a03255a7 169
aberk 0:7440a03255a7 170 //Keep going until we've moved the correct distance defined by the
aberk 0:7440a03255a7 171 //position set point. Take the absolute value of the pulses as we might
aberk 0:7440a03255a7 172 //be moving backwards.
aberk 0:7440a03255a7 173 while ((abs(leftPulses) < positionSetPoint) &&
aberk 0:7440a03255a7 174 (abs(rightPulses) < positionSetPoint)) {
aberk 0:7440a03255a7 175
aberk 0:7440a03255a7 176 //Get the current pulse count and subtract the previous one to
aberk 0:7440a03255a7 177 //calculate the current velocity in counts per second.
aberk 0:7440a03255a7 178 leftPulses = leftQei.getPulses();
aberk 0:7440a03255a7 179 leftVelocity = (leftPulses - leftPrevPulses) / RATE;
aberk 0:7440a03255a7 180 leftPrevPulses = leftPulses;
aberk 0:7440a03255a7 181 //Use the absolute value of velocity as the PID controller works
aberk 0:7440a03255a7 182 //in the % (unsigned) domain and will get confused with -ve values.
aberk 0:7440a03255a7 183 leftPid.setProcessValue(fabs(leftVelocity));
aberk 0:7440a03255a7 184 leftMotor.speed(leftPid.compute() * leftDirection);
aberk 0:7440a03255a7 185
aberk 0:7440a03255a7 186 rightPulses = rightQei.getPulses();
aberk 0:7440a03255a7 187 rightVelocity = (rightPulses - rightPrevPulses) / RATE;
aberk 0:7440a03255a7 188 rightPrevPulses = rightPulses;
aberk 0:7440a03255a7 189 rightPid.setProcessValue(fabs(rightVelocity));
aberk 0:7440a03255a7 190 rightMotor.speed(rightPid.compute() * rightDirection);
aberk 0:7440a03255a7 191
aberk 0:7440a03255a7 192 i++;
aberk 0:7440a03255a7 193
aberk 0:7440a03255a7 194 wait(RATE);
aberk 0:7440a03255a7 195
aberk 0:7440a03255a7 196 }
aberk 0:7440a03255a7 197
aberk 0:7440a03255a7 198 leftMotor.brake();
aberk 0:7440a03255a7 199 rightMotor.brake();
aberk 0:7440a03255a7 200
aberk 0:7440a03255a7 201 endHeading = imu.getYaw();
aberk 0:7440a03255a7 202
aberk 0:7440a03255a7 203 }
aberk 0:7440a03255a7 204 //turn command.
aberk 0:7440a03255a7 205 else if (strcmp(cmd0, "turn") == 0) {
aberk 0:7440a03255a7 206
aberk 0:7440a03255a7 207 //The PID controller works in the % (unsigned) domain, so we'll
aberk 0:7440a03255a7 208 //need to multiply the output by -1 for the motor which needs
aberk 0:7440a03255a7 209 //to go "backwards".
aberk 0:7440a03255a7 210 int leftDirection = 1;
aberk 0:7440a03255a7 211 int rightDirection = 1;
aberk 0:7440a03255a7 212 headingSetPoint = length + (endHeading - startHeading);
aberk 0:7440a03255a7 213
aberk 0:7440a03255a7 214 //In case the rover tries to [pointlessly] turn >360 degrees.
aberk 0:7440a03255a7 215 if (headingSetPoint > 359.8) {
aberk 0:7440a03255a7 216
aberk 0:7440a03255a7 217 headingSetPoint -= 359.8;
aberk 0:7440a03255a7 218
aberk 0:7440a03255a7 219 }
aberk 0:7440a03255a7 220
aberk 0:7440a03255a7 221 //Set up the right previous heading for the initial degreesTurned
aberk 0:7440a03255a7 222 //calculation.
aberk 0:7440a03255a7 223 prevHeading = fabs(imu.getYaw());
aberk 0:7440a03255a7 224
aberk 0:7440a03255a7 225 if (strcmp(cmd1, "left") == 0) {
aberk 0:7440a03255a7 226
aberk 0:7440a03255a7 227 //When turning left, the left motor needs to go backwards
aberk 0:7440a03255a7 228 //while the right motor goes forwards.
aberk 0:7440a03255a7 229 leftDirection = -1;
aberk 0:7440a03255a7 230
aberk 0:7440a03255a7 231 } else if (strcmp(cmd1, "right") == 0) {
aberk 0:7440a03255a7 232
aberk 0:7440a03255a7 233 //When turning right, the right motor needs to go backwards
aberk 0:7440a03255a7 234 //while the left motor goes forwards.
aberk 0:7440a03255a7 235 rightDirection = -1;
aberk 0:7440a03255a7 236
aberk 0:7440a03255a7 237 }
aberk 0:7440a03255a7 238
aberk 0:7440a03255a7 239 //Turn slowly to ensure we don't overshoot the angle by missing
aberk 0:7440a03255a7 240 //the relatively slow readings [in comparison to the PID loop speed]
aberk 0:7440a03255a7 241 //from the IMU.
aberk 0:7440a03255a7 242 leftPid.setSetPoint(500);
aberk 0:7440a03255a7 243 rightPid.setSetPoint(500);
aberk 0:7440a03255a7 244
aberk 0:7440a03255a7 245 //Keep turning until we've moved through the correct angle as defined
aberk 0:7440a03255a7 246 //by the heading set point.
aberk 0:7440a03255a7 247 while (degreesTurned < headingSetPoint) {
aberk 0:7440a03255a7 248
aberk 0:7440a03255a7 249 //Get the new heading and subtract the previous heading to
aberk 0:7440a03255a7 250 //determine how many more degrees we've moved through.
aberk 0:7440a03255a7 251 //As we're only interested in the relative angle (as opposed to
aberk 0:7440a03255a7 252 //absolute) we'll take the absolute value of the heading to avoid
aberk 0:7440a03255a7 253 //any nastiness when trying to calculate angles as they jump from
aberk 0:7440a03255a7 254 //179.9 to -179.9 degrees.
aberk 0:7440a03255a7 255 heading = fabs(imu.getYaw());
aberk 0:7440a03255a7 256 degreesTurned += fabs(heading - prevHeading);
aberk 0:7440a03255a7 257 prevHeading = heading;
aberk 0:7440a03255a7 258
aberk 0:7440a03255a7 259 //Get the current pulse count and subtract the previous one to
aberk 0:7440a03255a7 260 //calculate the current velocity in counts per second.
aberk 0:7440a03255a7 261 leftPulses = leftQei.getPulses();
aberk 0:7440a03255a7 262 leftVelocity = (leftPulses - leftPrevPulses) / RATE;
aberk 0:7440a03255a7 263 leftPrevPulses = leftPulses;
aberk 0:7440a03255a7 264 //Use the absolute value of velocity as the PID controller works
aberk 0:7440a03255a7 265 //in the % (unsigned) domain and will get confused with -ve values.
aberk 0:7440a03255a7 266 leftPid.setProcessValue(fabs(leftVelocity));
aberk 0:7440a03255a7 267 leftMotor.speed(leftPid.compute() * leftDirection);
aberk 0:7440a03255a7 268
aberk 0:7440a03255a7 269 rightPulses = rightQei.getPulses();
aberk 0:7440a03255a7 270 rightVelocity = (rightPulses - rightPrevPulses) / RATE;
aberk 0:7440a03255a7 271 rightPrevPulses = rightPulses;
aberk 0:7440a03255a7 272 rightPid.setProcessValue(fabs(rightVelocity));
aberk 0:7440a03255a7 273 rightMotor.speed(rightPid.compute() * rightDirection);
aberk 0:7440a03255a7 274
aberk 0:7440a03255a7 275 wait(RATE);
aberk 0:7440a03255a7 276
aberk 0:7440a03255a7 277 }
aberk 0:7440a03255a7 278
aberk 0:7440a03255a7 279 leftMotor.brake();
aberk 0:7440a03255a7 280 rightMotor.brake();
aberk 0:7440a03255a7 281
aberk 0:7440a03255a7 282 }
aberk 0:7440a03255a7 283
aberk 0:7440a03255a7 284 //Reset working variables.
aberk 0:7440a03255a7 285 leftQei.reset();
aberk 0:7440a03255a7 286 rightQei.reset();
aberk 0:7440a03255a7 287
aberk 0:7440a03255a7 288 leftPulses = 0;
aberk 0:7440a03255a7 289 leftPrevPulses = 0;
aberk 0:7440a03255a7 290 leftVelocity = 0.0;
aberk 0:7440a03255a7 291 leftMotor.speed(0.0);
aberk 0:7440a03255a7 292
aberk 0:7440a03255a7 293 rightPulses = 0;
aberk 0:7440a03255a7 294 rightPrevPulses = 0;
aberk 0:7440a03255a7 295 rightVelocity = 0.0;
aberk 0:7440a03255a7 296 rightMotor.speed(0.0);
aberk 0:7440a03255a7 297
aberk 0:7440a03255a7 298 leftPid.setSetPoint(0.0);
aberk 0:7440a03255a7 299 leftPid.setProcessValue(0.0);
aberk 0:7440a03255a7 300 rightPid.setSetPoint(0.0);
aberk 0:7440a03255a7 301 rightPid.setProcessValue(0.0);
aberk 0:7440a03255a7 302
aberk 0:7440a03255a7 303 positionSetPoint = 0.0;
aberk 0:7440a03255a7 304 headingSetPoint = 0.0;
aberk 0:7440a03255a7 305 heading = 0.0;
aberk 0:7440a03255a7 306 prevHeading = 0.0;
aberk 0:7440a03255a7 307 degreesTurned = 0.0;
aberk 0:7440a03255a7 308
aberk 0:7440a03255a7 309 }
aberk 0:7440a03255a7 310
aberk 0:7440a03255a7 311 int main() {
aberk 0:7440a03255a7 312
aberk 0:7440a03255a7 313 pc.printf("Starting mbed rover test...\n");
aberk 0:7440a03255a7 314
aberk 0:7440a03255a7 315 initializeMotors();
aberk 0:7440a03255a7 316 initializePid();
aberk 0:7440a03255a7 317
aberk 0:7440a03255a7 318 //Some delay before we start moving.
aberk 0:7440a03255a7 319 wait(5);
aberk 0:7440a03255a7 320
aberk 0:7440a03255a7 321 //Open the command script.
aberk 0:7440a03255a7 322 FILE* commands = fopen("/local/commands.txt", "r");
aberk 0:7440a03255a7 323
aberk 0:7440a03255a7 324 //Check if we were successful in opening the command script.
aberk 0:7440a03255a7 325 if (commands == NULL) {
aberk 0:7440a03255a7 326 pc.printf("Could not open command file...\n");
aberk 0:7440a03255a7 327 exit(EXIT_FAILURE);
aberk 0:7440a03255a7 328 } else {
aberk 0:7440a03255a7 329 pc.printf("Succesfully opened command file!\n");
aberk 0:7440a03255a7 330 }
aberk 0:7440a03255a7 331
aberk 0:7440a03255a7 332 //While there's another line to read from the command script.
aberk 0:7440a03255a7 333 while (fscanf(commands, "%s%s%f", cmd0, cmd1, &cmd2) >= 0) {
aberk 0:7440a03255a7 334
aberk 0:7440a03255a7 335 pc.printf("%s %s %f\n", cmd0, cmd1, cmd2);
aberk 0:7440a03255a7 336
aberk 0:7440a03255a7 337 processCommand(cmd0, cmd1, cmd2);
aberk 0:7440a03255a7 338
aberk 0:7440a03255a7 339 wait(1);
aberk 0:7440a03255a7 340
aberk 0:7440a03255a7 341 }
aberk 0:7440a03255a7 342
aberk 0:7440a03255a7 343 fclose(commands);
aberk 0:7440a03255a7 344
aberk 0:7440a03255a7 345 }