Justin Jordan / Mbed 2 deprecated Line_Following_Bot_Pololu

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

Dependencies:   MAX14871_Shield mbed

main.cpp@1:b928ca54cd1a, 2015-12-17 (annotated)

Committer:
j3
Date:
Thu Dec 17 02:29:28 2015 +0000
Revision:
1:b928ca54cd1a
Parent:
0:d2693f27f344
Child:
2:2c35ad38bf00
Just need to tune loop

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 1:b928ca54cd1a 57 //set PID terms to 0 if not used/needed
j3 1:b928ca54cd1a 58 const int32_t KP = 7;
j3 1:b928ca54cd1a 59 const int32_t KI = 0;
j3 1:b928ca54cd1a 60 const int32_t KD = 0;
j3 1:b928ca54cd1a 61 const int32_t TARGET_DUTY_CYCLE = 50; //starts bot off at 80% duty cycle
j3 0:d2693f27f344 62
j3 0:d2693f27f344 63 //raw sensor data
j3 0:d2693f27f344 64 uint8_t ir_val = 0;
j3 0:d2693f27f344 65
j3 1:b928ca54cd1a 66 //raw sensor data scaled
j3 1:b928ca54cd1a 67 //to a useable range for
j3 1:b928ca54cd1a 68 //error signal, SP - feedback raw sensor ir_val
j3 1:b928ca54cd1a 69 const uint8_t ERROR_SIGNAL_P7 = 0xFE; //b11111110
j3 1:b928ca54cd1a 70 const uint8_t ERROR_SIGNAL_P6 = 0xFC; //b11111100
j3 1:b928ca54cd1a 71 const uint8_t ERROR_SIGNAL_P5 = 0xFD; //b11111101
j3 1:b928ca54cd1a 72 const uint8_t ERROR_SIGNAL_P4 = 0xF9; //b11111001
j3 1:b928ca54cd1a 73 const uint8_t ERROR_SIGNAL_P3 = 0xFB; //b11111011
j3 1:b928ca54cd1a 74 const uint8_t ERROR_SIGNAL_P2 = 0xF3; //b11110011
j3 1:b928ca54cd1a 75 const uint8_t ERROR_SIGNAL_P1 = 0xF7; //b11110111
j3 1:b928ca54cd1a 76 const uint8_t ERROR_SIGNAL_00 = 0xE7; //b11100111, feedback = SP
j3 1:b928ca54cd1a 77 const uint8_t ERROR_SIGNAL_N1 = 0xEF; //b11101111
j3 1:b928ca54cd1a 78 const uint8_t ERROR_SIGNAL_N2 = 0xCF; //b11001111
j3 1:b928ca54cd1a 79 const uint8_t ERROR_SIGNAL_N3 = 0xDF; //b11011111
j3 1:b928ca54cd1a 80 const uint8_t ERROR_SIGNAL_N4 = 0x9F; //b10011111
j3 1:b928ca54cd1a 81 const uint8_t ERROR_SIGNAL_N5 = 0xBF; //b10111111
j3 1:b928ca54cd1a 82 const uint8_t ERROR_SIGNAL_N6 = 0x3F; //b00111111
j3 1:b928ca54cd1a 83 const uint8_t ERROR_SIGNAL_N7 = 0x7F; //b01111111
j3 0:d2693f27f344 84
j3 1:b928ca54cd1a 85 //special case error signals, 90 degree turns
j3 1:b928ca54cd1a 86 const uint8_t ERROR_LT0 = 0xE0; //b11100000
j3 1:b928ca54cd1a 87 const uint8_t ERROR_LT1 = 0xF0; //b11110000
j3 1:b928ca54cd1a 88 const uint8_t ERROR_LT2 = 0xF8; //b11111000
j3 1:b928ca54cd1a 89 const uint8_t ERROR_RT0 = 0x07; //b00000111
j3 1:b928ca54cd1a 90 const uint8_t ERROR_RT1 = 0x0F; //b00001111
j3 1:b928ca54cd1a 91 const uint8_t ERROR_RT2 = 0x1F; //b00011111
j3 1:b928ca54cd1a 92
j3 1:b928ca54cd1a 93 int32_t current_error = 0;
j3 1:b928ca54cd1a 94 int32_t previous_error = 0;
j3 1:b928ca54cd1a 95
j3 1:b928ca54cd1a 96 int32_t integral = 0;
j3 1:b928ca54cd1a 97 int32_t derivative = 0;
j3 1:b928ca54cd1a 98
j3 0:d2693f27f344 99
j3 0:d2693f27f344 100 for(;;)
j3 0:d2693f27f344 101 {
j3 0:d2693f27f344 102 //wait for start_stop button press
j3 0:d2693f27f344 103 while(!run);
j3 0:d2693f27f344 104
j3 1:b928ca54cd1a 105 //mode is set to forward, but duty cycle is still 0
j3 0:d2693f27f344 106 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 107 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 108
j3 0:d2693f27f344 109 while(run)
j3 0:d2693f27f344 110 {
j3 1:b928ca54cd1a 111 loop_pulse = !loop_pulse;
j3 1:b928ca54cd1a 112
j3 0:d2693f27f344 113 ir_val = ~(ir_bus.read());
j3 0:d2693f27f344 114
j3 0:d2693f27f344 115 //scale feedback
j3 0:d2693f27f344 116 switch(ir_val)
j3 0:d2693f27f344 117 {
j3 1:b928ca54cd1a 118 case(ERROR_SIGNAL_P7):
j3 1:b928ca54cd1a 119 current_error = 7;
j3 0:d2693f27f344 120 break;
j3 1:b928ca54cd1a 121
j3 1:b928ca54cd1a 122 case(ERROR_SIGNAL_P6):
j3 1:b928ca54cd1a 123 current_error = 6;
j3 0:d2693f27f344 124 break;
j3 0:d2693f27f344 125
j3 1:b928ca54cd1a 126 case(ERROR_SIGNAL_P5):
j3 1:b928ca54cd1a 127 current_error = 5;
j3 0:d2693f27f344 128 break;
j3 0:d2693f27f344 129
j3 1:b928ca54cd1a 130 case(ERROR_SIGNAL_P4):
j3 1:b928ca54cd1a 131 current_error = 4;
j3 1:b928ca54cd1a 132 break;
j3 1:b928ca54cd1a 133
j3 1:b928ca54cd1a 134 case(ERROR_SIGNAL_P3):
j3 1:b928ca54cd1a 135 current_error = 3;
j3 0:d2693f27f344 136 break;
j3 0:d2693f27f344 137
j3 1:b928ca54cd1a 138 case(ERROR_SIGNAL_P2):
j3 1:b928ca54cd1a 139 current_error = 2;
j3 0:d2693f27f344 140 break;
j3 0:d2693f27f344 141
j3 1:b928ca54cd1a 142 case(ERROR_SIGNAL_P1):
j3 1:b928ca54cd1a 143 current_error = 1;
j3 0:d2693f27f344 144 break;
j3 0:d2693f27f344 145
j3 1:b928ca54cd1a 146 case(ERROR_SIGNAL_00):
j3 1:b928ca54cd1a 147 current_error = 0;
j3 0:d2693f27f344 148 break;
j3 0:d2693f27f344 149
j3 1:b928ca54cd1a 150 case(ERROR_SIGNAL_N1):
j3 1:b928ca54cd1a 151 current_error = -1;
j3 0:d2693f27f344 152 break;
j3 0:d2693f27f344 153
j3 1:b928ca54cd1a 154 case(ERROR_SIGNAL_N2):
j3 1:b928ca54cd1a 155 current_error = -2;
j3 0:d2693f27f344 156 break;
j3 0:d2693f27f344 157
j3 1:b928ca54cd1a 158 case(ERROR_SIGNAL_N3):
j3 1:b928ca54cd1a 159 current_error = -3;
j3 0:d2693f27f344 160 break;
j3 0:d2693f27f344 161
j3 1:b928ca54cd1a 162 case(ERROR_SIGNAL_N4):
j3 1:b928ca54cd1a 163 current_error = -4;
j3 0:d2693f27f344 164 break;
j3 0:d2693f27f344 165
j3 1:b928ca54cd1a 166 case(ERROR_SIGNAL_N5):
j3 1:b928ca54cd1a 167 current_error = -5;
j3 0:d2693f27f344 168 break;
j3 0:d2693f27f344 169
j3 1:b928ca54cd1a 170 case(ERROR_SIGNAL_N6):
j3 1:b928ca54cd1a 171 current_error = -6;
j3 0:d2693f27f344 172 break;
j3 0:d2693f27f344 173
j3 1:b928ca54cd1a 174 case(ERROR_SIGNAL_N7):
j3 1:b928ca54cd1a 175 current_error = -7;
j3 0:d2693f27f344 176 break;
j3 1:b928ca54cd1a 177
j3 1:b928ca54cd1a 178 case(ERROR_LT0):
j3 1:b928ca54cd1a 179 current_error = 7;
j3 0:d2693f27f344 180 break;
j3 0:d2693f27f344 181
j3 1:b928ca54cd1a 182 case(ERROR_LT1):
j3 1:b928ca54cd1a 183 current_error = 7;
j3 0:d2693f27f344 184 break;
j3 0:d2693f27f344 185
j3 1:b928ca54cd1a 186 case(ERROR_LT2):
j3 1:b928ca54cd1a 187 current_error = 7;
j3 0:d2693f27f344 188 break;
j3 1:b928ca54cd1a 189
j3 1:b928ca54cd1a 190 case(ERROR_RT0):
j3 1:b928ca54cd1a 191 current_error = -7;
j3 0:d2693f27f344 192 break;
j3 0:d2693f27f344 193
j3 1:b928ca54cd1a 194 case(ERROR_RT1):
j3 1:b928ca54cd1a 195 current_error = -7;
j3 0:d2693f27f344 196 break;
j3 0:d2693f27f344 197
j3 1:b928ca54cd1a 198 case(ERROR_RT2):
j3 1:b928ca54cd1a 199 current_error = -7;
j3 0:d2693f27f344 200 break;
j3 1:b928ca54cd1a 201
j3 1:b928ca54cd1a 202 default:
j3 1:b928ca54cd1a 203 //do circles because I am a lost bot
j3 1:b928ca54cd1a 204 current_error = 7;
j3 1:b928ca54cd1a 205 break;
j3 0:d2693f27f344 206 }
j3 0:d2693f27f344 207
j3 0:d2693f27f344 208
j3 0:d2693f27f344 209 //get integral term, if statement helps w/wind-up
j3 0:d2693f27f344 210 if(current_error == 0)
j3 0:d2693f27f344 211 {
j3 0:d2693f27f344 212 integral = 0;
j3 0:d2693f27f344 213 }
j3 0:d2693f27f344 214 else if(((current_error < 0) && (previous_error > 0)) ||
j3 0:d2693f27f344 215 ((current_error > 0) && (previous_error < 0)))
j3 0:d2693f27f344 216 {
j3 0:d2693f27f344 217 integral = 0;
j3 0:d2693f27f344 218 }
j3 0:d2693f27f344 219 else
j3 0:d2693f27f344 220 {
j3 0:d2693f27f344 221 integral = (integral + current_error);
j3 0:d2693f27f344 222 }
j3 0:d2693f27f344 223
j3 0:d2693f27f344 224 //get derivative term
j3 0:d2693f27f344 225 derivative = (current_error - previous_error);
j3 0:d2693f27f344 226
j3 0:d2693f27f344 227 //save current error for next loop
j3 0:d2693f27f344 228 previous_error = current_error;
j3 0:d2693f27f344 229
j3 0:d2693f27f344 230 //get new duty cycle for right motor
j3 1:b928ca54cd1a 231 r_duty_cycle = (TARGET_DUTY_CYCLE - (KP*current_error + KI*integral + KD*derivative));
j3 1:b928ca54cd1a 232 //clamp to limits
j3 1:b928ca54cd1a 233 r_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, r_duty_cycle );
j3 1:b928ca54cd1a 234
j3 1:b928ca54cd1a 235
j3 1:b928ca54cd1a 236 //get new duty cycle for left motor
j3 1:b928ca54cd1a 237 l_duty_cycle = (TARGET_DUTY_CYCLE + (KP*current_error + KI*integral + KD*derivative));
j3 1:b928ca54cd1a 238 //clamp to limits
j3 1:b928ca54cd1a 239 l_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, l_duty_cycle );
j3 1:b928ca54cd1a 240
j3 1:b928ca54cd1a 241
j3 1:b928ca54cd1a 242 //update direction and duty cycle for right motor
j3 1:b928ca54cd1a 243 if(r_duty_cycle < 0)
j3 0:d2693f27f344 244 {
j3 1:b928ca54cd1a 245 r_duty_cycle = (r_duty_cycle * -1);
j3 1:b928ca54cd1a 246
j3 1:b928ca54cd1a 247 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::REVERSE);
j3 1:b928ca54cd1a 248 motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
j3 0:d2693f27f344 249 }
j3 1:b928ca54cd1a 250 else
j3 0:d2693f27f344 251 {
j3 1:b928ca54cd1a 252 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 1:b928ca54cd1a 253 motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
j3 0:d2693f27f344 254 }
j3 0:d2693f27f344 255
j3 1:b928ca54cd1a 256 //update direction and duty cycle for left motor
j3 1:b928ca54cd1a 257 if(l_duty_cycle < 0)
j3 0:d2693f27f344 258 {
j3 1:b928ca54cd1a 259 l_duty_cycle = (l_duty_cycle * -1);
j3 1:b928ca54cd1a 260
j3 1:b928ca54cd1a 261 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::REVERSE);
j3 1:b928ca54cd1a 262 motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
j3 0:d2693f27f344 263 }
j3 1:b928ca54cd1a 264 else
j3 0:d2693f27f344 265 {
j3 1:b928ca54cd1a 266 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 1:b928ca54cd1a 267 motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
j3 1:b928ca54cd1a 268 }
j3 0:d2693f27f344 269 }
j3 0:d2693f27f344 270
j3 0:d2693f27f344 271 //shutdown
j3 1:b928ca54cd1a 272 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 273 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 274
j3 1:b928ca54cd1a 275 //reset all data to initial state
j3 1:b928ca54cd1a 276 r_duty_cycle = 0;
j3 1:b928ca54cd1a 277 l_duty_cycle = 0;
j3 1:b928ca54cd1a 278
j3 1:b928ca54cd1a 279 current_error = 0;
j3 1:b928ca54cd1a 280 previous_error = current_error;
j3 1:b928ca54cd1a 281
j3 1:b928ca54cd1a 282 integral = 0;
j3 1:b928ca54cd1a 283 derivative = 0;
j3 0:d2693f27f344 284 }
j3 0:d2693f27f344 285 }
j3 0:d2693f27f344 286
j3 0:d2693f27f344 287
j3 1:b928ca54cd1a 288 int32_t clamp_duty_cycle(int32_t max, int32_t min, int32_t dc)
j3 0:d2693f27f344 289 {
j3 1:b928ca54cd1a 290 if(dc > max)
j3 0:d2693f27f344 291 {
j3 1:b928ca54cd1a 292 dc = max;
j3 1:b928ca54cd1a 293 }
j3 1:b928ca54cd1a 294
j3 1:b928ca54cd1a 295 if(dc < min)
j3 0:d2693f27f344 296 {
j3 1:b928ca54cd1a 297 dc = min;
j3 1:b928ca54cd1a 298 }
j3 1:b928ca54cd1a 299
j3 1:b928ca54cd1a 300 return(dc);
j3 1:b928ca54cd1a 301 }