Line following bot using MAXREFDES89, MAX32600MBED, and Pololu QTR-RC sensor

Dependencies:   MAX14871_Shield mbed

Committer:
j3
Date:
Mon Dec 21 00:40:11 2015 +0000
Revision:
3:d268f6e06b7a
Parent:
2:2c35ad38bf00
Child:
4:eb6d6d25355c
still tinkering

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j3 0:d2693f27f344 1 /**********************************************************************
j3 0:d2693f27f344 2 * Simple line following bot with PID to demo MAXREFDES89#
j3 0:d2693f27f344 3 * PID feedback comes from infrared led sensor from Parallax
j3 0:d2693f27f344 4 * https://www.parallax.com/product/28034
j3 1:b928ca54cd1a 5 *
j3 1:b928ca54cd1a 6 * The following link is a good example for a PID line follower
j3 1:b928ca54cd1a 7 *
j3 1:b928ca54cd1a 8 * http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html
j3 1:b928ca54cd1a 9 *
j3 0:d2693f27f344 10 **********************************************************************/
j3 0:d2693f27f344 11
j3 0:d2693f27f344 12 #include "mbed.h"
j3 0:d2693f27f344 13 #include "max14871_shield.h"
j3 0:d2693f27f344 14
j3 0:d2693f27f344 15
j3 1:b928ca54cd1a 16 //helper functions to make code in main loop clearer/readable
j3 1:b928ca54cd1a 17 int32_t clamp_duty_cycle(int32_t max, int32_t min, int32_t dc);
j3 0:d2693f27f344 18
j3 1:b928ca54cd1a 19
j3 1:b928ca54cd1a 20 //state variables for ISR
j3 0:d2693f27f344 21 bool run = false;
j3 1:b928ca54cd1a 22 BusOut start_stop_led(D6, D7);
j3 0:d2693f27f344 23
j3 1:b928ca54cd1a 24 //Input to trigger interrupt off of
j3 0:d2693f27f344 25 InterruptIn start_stop_button(SW3);
j3 0:d2693f27f344 26
j3 1:b928ca54cd1a 27 //callback fx for ISR
j3 0:d2693f27f344 28 void start_stop()
j3 0:d2693f27f344 29 {
j3 0:d2693f27f344 30 run = !run;
j3 1:b928ca54cd1a 31 start_stop_led = (start_stop_led ^ 3);
j3 0:d2693f27f344 32 }
j3 0:d2693f27f344 33
j3 0:d2693f27f344 34
j3 0:d2693f27f344 35 int main(void)
j3 0:d2693f27f344 36 {
j3 0:d2693f27f344 37 start_stop_button.fall(&start_stop);
j3 0:d2693f27f344 38
j3 1:b928ca54cd1a 39 start_stop_led = 2; // D7 on D6 off = red
j3 1:b928ca54cd1a 40
j3 0:d2693f27f344 41 BusIn ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
j3 0:d2693f27f344 42 DigitalOut ir_bus_enable(D3, 1); //active high enable
j3 0:d2693f27f344 43
j3 1:b928ca54cd1a 44 DigitalOut loop_pulse(D8, 0);
j3 1:b928ca54cd1a 45
j3 0:d2693f27f344 46 Max14871_Shield motor_shield(D14, D15, true);// Default config
j3 0:d2693f27f344 47
j3 0:d2693f27f344 48 Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4;
j3 0:d2693f27f344 49 Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3;
j3 0:d2693f27f344 50
j3 1:b928ca54cd1a 51 int32_t r_duty_cycle = 0;
j3 1:b928ca54cd1a 52 int32_t l_duty_cycle = 0;
j3 1:b928ca54cd1a 53
j3 1:b928ca54cd1a 54 const int32_t MAX_DUTY_CYCLE = 100;
j3 1:b928ca54cd1a 55 const int32_t MIN_DUTY_CYCLE = -100;
j3 0:d2693f27f344 56
j3 3:d268f6e06b7a 57
j3 3:d268f6e06b7a 58 /***************************************************
j3 3:d268f6e06b7a 59 | Control Type | KP | KI | KD |
j3 3:d268f6e06b7a 60 |---------------|--------|------------|------------|
j3 3:d268f6e06b7a 61 | P | 0.5Kc | 0 | 0 |
j3 3:d268f6e06b7a 62 |---------------------------------------------------
j3 3:d268f6e06b7a 63 | PI | 0.45Kc | 1.2KpdT/Pc | 0 |
j3 3:d268f6e06b7a 64 |---------------|--------|------------|------------|
j3 3:d268f6e06b7a 65 | PD | 0.80Kc | 0 | KpPc/(8dT) |
j3 3:d268f6e06b7a 66 |---------------|--------|------------|------------|
j3 3:d268f6e06b7a 67 | PID | 0.60Kc | 2KpdT/Pc | KpPc/(8dT) |
j3 3:d268f6e06b7a 68 ***************************************************/
j3 3:d268f6e06b7a 69
j3 3:d268f6e06b7a 70 //Kc = critical gain, gain with just P control at which system becomes marginally stable
j3 3:d268f6e06b7a 71 //Pc = period of oscillation for previous scenario.
j3 3:d268f6e06b7a 72
j3 1:b928ca54cd1a 73 //set PID terms to 0 if not used/needed
j3 2:2c35ad38bf00 74 const int32_t KP = 16;
j3 1:b928ca54cd1a 75 const int32_t KI = 0;
j3 1:b928ca54cd1a 76 const int32_t KD = 0;
j3 3:d268f6e06b7a 77 const int32_t TARGET_DUTY_CYCLE = 50;
j3 0:d2693f27f344 78
j3 0:d2693f27f344 79 //raw sensor data
j3 0:d2693f27f344 80 uint8_t ir_val = 0;
j3 0:d2693f27f344 81
j3 1:b928ca54cd1a 82 //raw sensor data scaled
j3 1:b928ca54cd1a 83 //to a useable range for
j3 1:b928ca54cd1a 84 //error signal, SP - feedback raw sensor ir_val
j3 1:b928ca54cd1a 85 const uint8_t ERROR_SIGNAL_P7 = 0xFE; //b11111110
j3 1:b928ca54cd1a 86 const uint8_t ERROR_SIGNAL_P6 = 0xFC; //b11111100
j3 1:b928ca54cd1a 87 const uint8_t ERROR_SIGNAL_P5 = 0xFD; //b11111101
j3 1:b928ca54cd1a 88 const uint8_t ERROR_SIGNAL_P4 = 0xF9; //b11111001
j3 1:b928ca54cd1a 89 const uint8_t ERROR_SIGNAL_P3 = 0xFB; //b11111011
j3 1:b928ca54cd1a 90 const uint8_t ERROR_SIGNAL_P2 = 0xF3; //b11110011
j3 1:b928ca54cd1a 91 const uint8_t ERROR_SIGNAL_P1 = 0xF7; //b11110111
j3 1:b928ca54cd1a 92 const uint8_t ERROR_SIGNAL_00 = 0xE7; //b11100111, feedback = SP
j3 1:b928ca54cd1a 93 const uint8_t ERROR_SIGNAL_N1 = 0xEF; //b11101111
j3 1:b928ca54cd1a 94 const uint8_t ERROR_SIGNAL_N2 = 0xCF; //b11001111
j3 1:b928ca54cd1a 95 const uint8_t ERROR_SIGNAL_N3 = 0xDF; //b11011111
j3 1:b928ca54cd1a 96 const uint8_t ERROR_SIGNAL_N4 = 0x9F; //b10011111
j3 1:b928ca54cd1a 97 const uint8_t ERROR_SIGNAL_N5 = 0xBF; //b10111111
j3 1:b928ca54cd1a 98 const uint8_t ERROR_SIGNAL_N6 = 0x3F; //b00111111
j3 1:b928ca54cd1a 99 const uint8_t ERROR_SIGNAL_N7 = 0x7F; //b01111111
j3 0:d2693f27f344 100
j3 1:b928ca54cd1a 101 //special case error signals, 90 degree turns
j3 1:b928ca54cd1a 102 const uint8_t ERROR_LT0 = 0xE0; //b11100000
j3 1:b928ca54cd1a 103 const uint8_t ERROR_LT1 = 0xF0; //b11110000
j3 1:b928ca54cd1a 104 const uint8_t ERROR_LT2 = 0xF8; //b11111000
j3 1:b928ca54cd1a 105 const uint8_t ERROR_RT0 = 0x07; //b00000111
j3 1:b928ca54cd1a 106 const uint8_t ERROR_RT1 = 0x0F; //b00001111
j3 1:b928ca54cd1a 107 const uint8_t ERROR_RT2 = 0x1F; //b00011111
j3 1:b928ca54cd1a 108
j3 3:d268f6e06b7a 109 //Lost bot error signal
j3 3:d268f6e06b7a 110 const uint8_t ERROR_OFF_TRACK = 0xFF; //b11111111
j3 3:d268f6e06b7a 111
j3 1:b928ca54cd1a 112 int32_t current_error = 0;
j3 1:b928ca54cd1a 113 int32_t previous_error = 0;
j3 1:b928ca54cd1a 114
j3 1:b928ca54cd1a 115 int32_t integral = 0;
j3 1:b928ca54cd1a 116 int32_t derivative = 0;
j3 1:b928ca54cd1a 117
j3 0:d2693f27f344 118
j3 0:d2693f27f344 119 for(;;)
j3 0:d2693f27f344 120 {
j3 0:d2693f27f344 121 //wait for start_stop button press
j3 0:d2693f27f344 122 while(!run);
j3 0:d2693f27f344 123
j3 1:b928ca54cd1a 124 //mode is set to forward, but duty cycle is still 0
j3 0:d2693f27f344 125 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 126 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 127
j3 0:d2693f27f344 128 while(run)
j3 0:d2693f27f344 129 {
j3 1:b928ca54cd1a 130 loop_pulse = !loop_pulse;
j3 1:b928ca54cd1a 131
j3 0:d2693f27f344 132 ir_val = ~(ir_bus.read());
j3 0:d2693f27f344 133
j3 0:d2693f27f344 134 //scale feedback
j3 0:d2693f27f344 135 switch(ir_val)
j3 0:d2693f27f344 136 {
j3 1:b928ca54cd1a 137 case(ERROR_SIGNAL_P7):
j3 1:b928ca54cd1a 138 current_error = 7;
j3 0:d2693f27f344 139 break;
j3 1:b928ca54cd1a 140
j3 1:b928ca54cd1a 141 case(ERROR_SIGNAL_P6):
j3 1:b928ca54cd1a 142 current_error = 6;
j3 0:d2693f27f344 143 break;
j3 0:d2693f27f344 144
j3 1:b928ca54cd1a 145 case(ERROR_SIGNAL_P5):
j3 1:b928ca54cd1a 146 current_error = 5;
j3 0:d2693f27f344 147 break;
j3 0:d2693f27f344 148
j3 1:b928ca54cd1a 149 case(ERROR_SIGNAL_P4):
j3 1:b928ca54cd1a 150 current_error = 4;
j3 1:b928ca54cd1a 151 break;
j3 1:b928ca54cd1a 152
j3 1:b928ca54cd1a 153 case(ERROR_SIGNAL_P3):
j3 1:b928ca54cd1a 154 current_error = 3;
j3 0:d2693f27f344 155 break;
j3 0:d2693f27f344 156
j3 1:b928ca54cd1a 157 case(ERROR_SIGNAL_P2):
j3 1:b928ca54cd1a 158 current_error = 2;
j3 0:d2693f27f344 159 break;
j3 0:d2693f27f344 160
j3 1:b928ca54cd1a 161 case(ERROR_SIGNAL_P1):
j3 1:b928ca54cd1a 162 current_error = 1;
j3 0:d2693f27f344 163 break;
j3 0:d2693f27f344 164
j3 1:b928ca54cd1a 165 case(ERROR_SIGNAL_00):
j3 1:b928ca54cd1a 166 current_error = 0;
j3 0:d2693f27f344 167 break;
j3 0:d2693f27f344 168
j3 1:b928ca54cd1a 169 case(ERROR_SIGNAL_N1):
j3 1:b928ca54cd1a 170 current_error = -1;
j3 0:d2693f27f344 171 break;
j3 0:d2693f27f344 172
j3 1:b928ca54cd1a 173 case(ERROR_SIGNAL_N2):
j3 1:b928ca54cd1a 174 current_error = -2;
j3 0:d2693f27f344 175 break;
j3 0:d2693f27f344 176
j3 1:b928ca54cd1a 177 case(ERROR_SIGNAL_N3):
j3 1:b928ca54cd1a 178 current_error = -3;
j3 0:d2693f27f344 179 break;
j3 0:d2693f27f344 180
j3 1:b928ca54cd1a 181 case(ERROR_SIGNAL_N4):
j3 1:b928ca54cd1a 182 current_error = -4;
j3 0:d2693f27f344 183 break;
j3 0:d2693f27f344 184
j3 1:b928ca54cd1a 185 case(ERROR_SIGNAL_N5):
j3 1:b928ca54cd1a 186 current_error = -5;
j3 0:d2693f27f344 187 break;
j3 0:d2693f27f344 188
j3 1:b928ca54cd1a 189 case(ERROR_SIGNAL_N6):
j3 1:b928ca54cd1a 190 current_error = -6;
j3 0:d2693f27f344 191 break;
j3 0:d2693f27f344 192
j3 1:b928ca54cd1a 193 case(ERROR_SIGNAL_N7):
j3 1:b928ca54cd1a 194 current_error = -7;
j3 0:d2693f27f344 195 break;
j3 1:b928ca54cd1a 196
j3 1:b928ca54cd1a 197 case(ERROR_LT0):
j3 1:b928ca54cd1a 198 current_error = 7;
j3 0:d2693f27f344 199 break;
j3 0:d2693f27f344 200
j3 1:b928ca54cd1a 201 case(ERROR_LT1):
j3 1:b928ca54cd1a 202 current_error = 7;
j3 0:d2693f27f344 203 break;
j3 0:d2693f27f344 204
j3 1:b928ca54cd1a 205 case(ERROR_LT2):
j3 1:b928ca54cd1a 206 current_error = 7;
j3 0:d2693f27f344 207 break;
j3 1:b928ca54cd1a 208
j3 1:b928ca54cd1a 209 case(ERROR_RT0):
j3 1:b928ca54cd1a 210 current_error = -7;
j3 0:d2693f27f344 211 break;
j3 0:d2693f27f344 212
j3 1:b928ca54cd1a 213 case(ERROR_RT1):
j3 1:b928ca54cd1a 214 current_error = -7;
j3 0:d2693f27f344 215 break;
j3 0:d2693f27f344 216
j3 1:b928ca54cd1a 217 case(ERROR_RT2):
j3 1:b928ca54cd1a 218 current_error = -7;
j3 0:d2693f27f344 219 break;
j3 3:d268f6e06b7a 220
j3 3:d268f6e06b7a 221 case(ERROR_OFF_TRACK):
j3 3:d268f6e06b7a 222 //do circles cause I am lost
j3 3:d268f6e06b7a 223 current_error = 7;
j3 3:d268f6e06b7a 224 break;
j3 1:b928ca54cd1a 225
j3 1:b928ca54cd1a 226 default:
j3 3:d268f6e06b7a 227 current_error = previous_error;
j3 1:b928ca54cd1a 228 break;
j3 0:d2693f27f344 229 }
j3 0:d2693f27f344 230
j3 0:d2693f27f344 231
j3 0:d2693f27f344 232 //get integral term, if statement helps w/wind-up
j3 0:d2693f27f344 233 if(current_error == 0)
j3 0:d2693f27f344 234 {
j3 0:d2693f27f344 235 integral = 0;
j3 0:d2693f27f344 236 }
j3 0:d2693f27f344 237 else if(((current_error < 0) && (previous_error > 0)) ||
j3 0:d2693f27f344 238 ((current_error > 0) && (previous_error < 0)))
j3 0:d2693f27f344 239 {
j3 0:d2693f27f344 240 integral = 0;
j3 0:d2693f27f344 241 }
j3 0:d2693f27f344 242 else
j3 0:d2693f27f344 243 {
j3 0:d2693f27f344 244 integral = (integral + current_error);
j3 0:d2693f27f344 245 }
j3 0:d2693f27f344 246
j3 0:d2693f27f344 247 //get derivative term
j3 0:d2693f27f344 248 derivative = (current_error - previous_error);
j3 0:d2693f27f344 249
j3 0:d2693f27f344 250 //save current error for next loop
j3 0:d2693f27f344 251 previous_error = current_error;
j3 0:d2693f27f344 252
j3 0:d2693f27f344 253 //get new duty cycle for right motor
j3 3:d268f6e06b7a 254 r_duty_cycle = (TARGET_DUTY_CYCLE - (KP*current_error + (KI*integral) + KD*derivative));
j3 1:b928ca54cd1a 255 //clamp to limits
j3 1:b928ca54cd1a 256 r_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, r_duty_cycle );
j3 1:b928ca54cd1a 257
j3 1:b928ca54cd1a 258
j3 1:b928ca54cd1a 259 //get new duty cycle for left motor
j3 3:d268f6e06b7a 260 l_duty_cycle = (TARGET_DUTY_CYCLE + (KP*current_error + (KI*integral) + KD*derivative));
j3 1:b928ca54cd1a 261 //clamp to limits
j3 1:b928ca54cd1a 262 l_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, l_duty_cycle );
j3 1:b928ca54cd1a 263
j3 1:b928ca54cd1a 264
j3 1:b928ca54cd1a 265 //update direction and duty cycle for right motor
j3 1:b928ca54cd1a 266 if(r_duty_cycle < 0)
j3 0:d2693f27f344 267 {
j3 1:b928ca54cd1a 268 r_duty_cycle = (r_duty_cycle * -1);
j3 1:b928ca54cd1a 269
j3 1:b928ca54cd1a 270 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::REVERSE);
j3 1:b928ca54cd1a 271 motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
j3 0:d2693f27f344 272 }
j3 1:b928ca54cd1a 273 else
j3 0:d2693f27f344 274 {
j3 1:b928ca54cd1a 275 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 1:b928ca54cd1a 276 motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
j3 0:d2693f27f344 277 }
j3 0:d2693f27f344 278
j3 1:b928ca54cd1a 279 //update direction and duty cycle for left motor
j3 1:b928ca54cd1a 280 if(l_duty_cycle < 0)
j3 0:d2693f27f344 281 {
j3 1:b928ca54cd1a 282 l_duty_cycle = (l_duty_cycle * -1);
j3 1:b928ca54cd1a 283
j3 1:b928ca54cd1a 284 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::REVERSE);
j3 1:b928ca54cd1a 285 motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
j3 0:d2693f27f344 286 }
j3 1:b928ca54cd1a 287 else
j3 0:d2693f27f344 288 {
j3 1:b928ca54cd1a 289 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 1:b928ca54cd1a 290 motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
j3 1:b928ca54cd1a 291 }
j3 0:d2693f27f344 292 }
j3 0:d2693f27f344 293
j3 0:d2693f27f344 294 //shutdown
j3 1:b928ca54cd1a 295 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 296 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 297
j3 1:b928ca54cd1a 298 //reset all data to initial state
j3 1:b928ca54cd1a 299 r_duty_cycle = 0;
j3 1:b928ca54cd1a 300 l_duty_cycle = 0;
j3 1:b928ca54cd1a 301
j3 1:b928ca54cd1a 302 current_error = 0;
j3 1:b928ca54cd1a 303 previous_error = current_error;
j3 1:b928ca54cd1a 304
j3 1:b928ca54cd1a 305 integral = 0;
j3 1:b928ca54cd1a 306 derivative = 0;
j3 0:d2693f27f344 307 }
j3 0:d2693f27f344 308 }
j3 0:d2693f27f344 309
j3 0:d2693f27f344 310
j3 1:b928ca54cd1a 311 int32_t clamp_duty_cycle(int32_t max, int32_t min, int32_t dc)
j3 0:d2693f27f344 312 {
j3 1:b928ca54cd1a 313 if(dc > max)
j3 0:d2693f27f344 314 {
j3 1:b928ca54cd1a 315 dc = max;
j3 1:b928ca54cd1a 316 }
j3 1:b928ca54cd1a 317
j3 1:b928ca54cd1a 318 if(dc < min)
j3 0:d2693f27f344 319 {
j3 1:b928ca54cd1a 320 dc = min;
j3 1:b928ca54cd1a 321 }
j3 1:b928ca54cd1a 322
j3 1:b928ca54cd1a 323 return(dc);
j3 1:b928ca54cd1a 324 }