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?
User | Revision | Line number | New 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 | } |