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@0:d2693f27f344, 2015-12-14 (annotated)

Committer:
j3
Date:
Mon Dec 14 00:03:54 2015 +0000
Revision:
0:d2693f27f344
Child:
1:b928ca54cd1a
initial commit, ported my code over from the prop and switched from using the HB-25 motor drivers to MAXREFDES89#.  Need to redue scaling for zones and 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 0:d2693f27f344 5 **********************************************************************/
j3 0:d2693f27f344 6
j3 0:d2693f27f344 7 #include "mbed.h"
j3 0:d2693f27f344 8 #include "max14871_shield.h"
j3 0:d2693f27f344 9
j3 0:d2693f27f344 10
j3 0:d2693f27f344 11 float ramp(float val, float val_old);
j3 0:d2693f27f344 12
j3 0:d2693f27f344 13 bool run = false;
j3 0:d2693f27f344 14 DigitalOut run_led(LED_GREEN, 1); //leds have inverted logic
j3 0:d2693f27f344 15 DigitalOut stop_led(LED_RED, 0);
j3 0:d2693f27f344 16
j3 0:d2693f27f344 17 InterruptIn start_stop_button(SW3);
j3 0:d2693f27f344 18
j3 0:d2693f27f344 19 void start_stop()
j3 0:d2693f27f344 20 {
j3 0:d2693f27f344 21 run = !run;
j3 0:d2693f27f344 22 run_led = !run_led;
j3 0:d2693f27f344 23 stop_led = !stop_led;
j3 0:d2693f27f344 24 }
j3 0:d2693f27f344 25
j3 0:d2693f27f344 26
j3 0:d2693f27f344 27 int main(void)
j3 0:d2693f27f344 28 {
j3 0:d2693f27f344 29 start_stop_button.fall(&start_stop);
j3 0:d2693f27f344 30
j3 0:d2693f27f344 31 BusIn ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
j3 0:d2693f27f344 32 DigitalOut ir_bus_enable(D3, 1); //active high enable
j3 0:d2693f27f344 33
j3 0:d2693f27f344 34 Max14871_Shield motor_shield(D14, D15, true);// Default config
j3 0:d2693f27f344 35
j3 0:d2693f27f344 36 Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4;
j3 0:d2693f27f344 37 Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3;
j3 0:d2693f27f344 38
j3 0:d2693f27f344 39 const float MAX_DUTY_CYCLE = 1.0;
j3 0:d2693f27f344 40 const float MIN_DUTY_CYCLE = 0.0;
j3 0:d2693f27f344 41
j3 0:d2693f27f344 42 //set point is for position relative to line, not duty cycle of pwm
j3 0:d2693f27f344 43 const float SP = 0.0;
j3 0:d2693f27f344 44 const float KP = 1.0;
j3 0:d2693f27f344 45 const float KI = 0.0;
j3 0:d2693f27f344 46 const float KD = 0.0;
j3 0:d2693f27f344 47 //const float OFFSET = 0;
j3 0:d2693f27f344 48
j3 0:d2693f27f344 49 //raw sensor data
j3 0:d2693f27f344 50 uint8_t ir_val = 0;
j3 0:d2693f27f344 51
j3 0:d2693f27f344 52 const uint8_t ZONE_8 = 0xFF;
j3 0:d2693f27f344 53 const uint8_t ZONE_7 = 0xFE;
j3 0:d2693f27f344 54 const uint8_t ZONE_6 = 0xFC;
j3 0:d2693f27f344 55 const uint8_t ZONE_5 = 0xFD;
j3 0:d2693f27f344 56 const uint8_t ZONE_4 = 0xF9;
j3 0:d2693f27f344 57 const uint8_t ZONE_3 = 0xFB;
j3 0:d2693f27f344 58 const uint8_t ZONE_2 = 0xF3;
j3 0:d2693f27f344 59 const uint8_t ZONE_1 = 0xF7;
j3 0:d2693f27f344 60 const uint8_t ZONE_0 = 0xE7;
j3 0:d2693f27f344 61 const uint8_t ZONE_N1 = 0xEF;
j3 0:d2693f27f344 62 const uint8_t ZONE_N2 = 0xCF;
j3 0:d2693f27f344 63 const uint8_t ZONE_N3 = 0xDF;
j3 0:d2693f27f344 64 const uint8_t ZONE_N4 = 0x9F;
j3 0:d2693f27f344 65 const uint8_t ZONE_N5 = 0xBF;
j3 0:d2693f27f344 66 const uint8_t ZONE_N6 = 0x3F;
j3 0:d2693f27f344 67 const uint8_t ZONE_N7 = 0x7F;
j3 0:d2693f27f344 68 const uint8_t ZONE_LT0 = 0xE0;
j3 0:d2693f27f344 69 const uint8_t ZONE_LT1 = 0xF0;
j3 0:d2693f27f344 70 const uint8_t ZONE_LT2 = 0xF8;
j3 0:d2693f27f344 71 const uint8_t ZONE_RT0 = 0x07;
j3 0:d2693f27f344 72 const uint8_t ZONE_RT1 = 0x0F;
j3 0:d2693f27f344 73 const uint8_t ZONE_RT2 = 0x1F;
j3 0:d2693f27f344 74
j3 0:d2693f27f344 75 float r_duty_cycle = 0.0;
j3 0:d2693f27f344 76 float r_duty_cycle_old = r_duty_cycle;
j3 0:d2693f27f344 77
j3 0:d2693f27f344 78 float l_duty_cycle = 0.0;
j3 0:d2693f27f344 79 float l_duty_cycle_old = l_duty_cycle;
j3 0:d2693f27f344 80
j3 0:d2693f27f344 81 float fb = 0;
j3 0:d2693f27f344 82 float current_error = 0;
j3 0:d2693f27f344 83 float previous_error = 0;
j3 0:d2693f27f344 84 float integral = 0;
j3 0:d2693f27f344 85 float derivative = 0;
j3 0:d2693f27f344 86
j3 0:d2693f27f344 87 for(;;)
j3 0:d2693f27f344 88 {
j3 0:d2693f27f344 89 //wait for start_stop button press
j3 0:d2693f27f344 90 while(!run);
j3 0:d2693f27f344 91
j3 0:d2693f27f344 92 //mode is set to forward, but duty cycle is still 0.0
j3 0:d2693f27f344 93 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 94 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 95
j3 0:d2693f27f344 96 while(run)
j3 0:d2693f27f344 97 {
j3 0:d2693f27f344 98 ir_val = ~(ir_bus.read());
j3 0:d2693f27f344 99
j3 0:d2693f27f344 100 //scale feedback
j3 0:d2693f27f344 101 switch(ir_val)
j3 0:d2693f27f344 102 {
j3 0:d2693f27f344 103 case(ZONE_8):
j3 0:d2693f27f344 104 if(fb < 0)
j3 0:d2693f27f344 105 {
j3 0:d2693f27f344 106 fb = -8;
j3 0:d2693f27f344 107 }
j3 0:d2693f27f344 108 else
j3 0:d2693f27f344 109 {
j3 0:d2693f27f344 110 fb = 8;
j3 0:d2693f27f344 111 }
j3 0:d2693f27f344 112 break;
j3 0:d2693f27f344 113
j3 0:d2693f27f344 114 case(ZONE_7):
j3 0:d2693f27f344 115 fb = 7;
j3 0:d2693f27f344 116 break;
j3 0:d2693f27f344 117
j3 0:d2693f27f344 118 case(ZONE_6):
j3 0:d2693f27f344 119 fb = 6;
j3 0:d2693f27f344 120 break;
j3 0:d2693f27f344 121
j3 0:d2693f27f344 122 case(ZONE_5):
j3 0:d2693f27f344 123 fb = 5;
j3 0:d2693f27f344 124 break;
j3 0:d2693f27f344 125
j3 0:d2693f27f344 126 case(ZONE_4):
j3 0:d2693f27f344 127 fb = 4;
j3 0:d2693f27f344 128 break;
j3 0:d2693f27f344 129
j3 0:d2693f27f344 130 case(ZONE_3):
j3 0:d2693f27f344 131 fb = 3;
j3 0:d2693f27f344 132 break;
j3 0:d2693f27f344 133
j3 0:d2693f27f344 134 case(ZONE_2):
j3 0:d2693f27f344 135 fb = 2;
j3 0:d2693f27f344 136 break;
j3 0:d2693f27f344 137
j3 0:d2693f27f344 138 case(ZONE_1):
j3 0:d2693f27f344 139 fb = 1;
j3 0:d2693f27f344 140 break;
j3 0:d2693f27f344 141
j3 0:d2693f27f344 142 case(ZONE_0):
j3 0:d2693f27f344 143 fb = 0;
j3 0:d2693f27f344 144 break;
j3 0:d2693f27f344 145
j3 0:d2693f27f344 146 case(ZONE_N1):
j3 0:d2693f27f344 147 fb = -1;
j3 0:d2693f27f344 148 break;
j3 0:d2693f27f344 149
j3 0:d2693f27f344 150 case(ZONE_N2):
j3 0:d2693f27f344 151 fb = -2;
j3 0:d2693f27f344 152 break;
j3 0:d2693f27f344 153
j3 0:d2693f27f344 154 case(ZONE_N3):
j3 0:d2693f27f344 155 fb = -3;
j3 0:d2693f27f344 156 break;
j3 0:d2693f27f344 157
j3 0:d2693f27f344 158 case(ZONE_N4):
j3 0:d2693f27f344 159 fb = -4;
j3 0:d2693f27f344 160 break;
j3 0:d2693f27f344 161
j3 0:d2693f27f344 162 case(ZONE_N5):
j3 0:d2693f27f344 163 fb = -5;
j3 0:d2693f27f344 164 break;
j3 0:d2693f27f344 165
j3 0:d2693f27f344 166 case(ZONE_N6):
j3 0:d2693f27f344 167 fb = -6;
j3 0:d2693f27f344 168 break;
j3 0:d2693f27f344 169
j3 0:d2693f27f344 170 case(ZONE_N7):
j3 0:d2693f27f344 171 fb = -7;
j3 0:d2693f27f344 172 break;
j3 0:d2693f27f344 173
j3 0:d2693f27f344 174 case(ZONE_LT0):
j3 0:d2693f27f344 175 fb = 8;
j3 0:d2693f27f344 176 break;
j3 0:d2693f27f344 177
j3 0:d2693f27f344 178 case(ZONE_LT1):
j3 0:d2693f27f344 179 fb = 8;
j3 0:d2693f27f344 180 break;
j3 0:d2693f27f344 181
j3 0:d2693f27f344 182 case(ZONE_LT2):
j3 0:d2693f27f344 183 fb = 8;
j3 0:d2693f27f344 184 break;
j3 0:d2693f27f344 185
j3 0:d2693f27f344 186 case(ZONE_RT0):
j3 0:d2693f27f344 187 fb = -8;
j3 0:d2693f27f344 188 break;
j3 0:d2693f27f344 189
j3 0:d2693f27f344 190 case(ZONE_RT1):
j3 0:d2693f27f344 191 fb = -8;
j3 0:d2693f27f344 192 break;
j3 0:d2693f27f344 193
j3 0:d2693f27f344 194 case(ZONE_RT2):
j3 0:d2693f27f344 195 fb = -8;
j3 0:d2693f27f344 196 break;
j3 0:d2693f27f344 197
j3 0:d2693f27f344 198 default:
j3 0:d2693f27f344 199 fb = fb;
j3 0:d2693f27f344 200 }
j3 0:d2693f27f344 201
j3 0:d2693f27f344 202 //get error signal
j3 0:d2693f27f344 203 current_error = SP-fb;
j3 0:d2693f27f344 204
j3 0:d2693f27f344 205 //get integral term, if statement helps w/wind-up
j3 0:d2693f27f344 206 if(current_error == 0)
j3 0:d2693f27f344 207 {
j3 0:d2693f27f344 208 integral = 0;
j3 0:d2693f27f344 209 }
j3 0:d2693f27f344 210 else if(((current_error < 0) && (previous_error > 0)) ||
j3 0:d2693f27f344 211 ((current_error > 0) && (previous_error < 0)))
j3 0:d2693f27f344 212 {
j3 0:d2693f27f344 213 integral = 0;
j3 0:d2693f27f344 214 }
j3 0:d2693f27f344 215 else
j3 0:d2693f27f344 216 {
j3 0:d2693f27f344 217 integral = (integral + current_error);
j3 0:d2693f27f344 218 }
j3 0:d2693f27f344 219
j3 0:d2693f27f344 220 //get derivative term
j3 0:d2693f27f344 221 derivative = (current_error - previous_error);
j3 0:d2693f27f344 222
j3 0:d2693f27f344 223 //save current error for next loop
j3 0:d2693f27f344 224 previous_error = current_error;
j3 0:d2693f27f344 225
j3 0:d2693f27f344 226 //get new duty cycle for right motor
j3 0:d2693f27f344 227 r_duty_cycle = ((KP*current_error) + (KI*integral) + (KD*derivative));
j3 0:d2693f27f344 228 if(r_duty_cycle > MAX_DUTY_CYCLE)
j3 0:d2693f27f344 229 {
j3 0:d2693f27f344 230 r_duty_cycle = MAX_DUTY_CYCLE;
j3 0:d2693f27f344 231 }
j3 0:d2693f27f344 232
j3 0:d2693f27f344 233 if(r_duty_cycle < MIN_DUTY_CYCLE)
j3 0:d2693f27f344 234 {
j3 0:d2693f27f344 235 r_duty_cycle = MIN_DUTY_CYCLE;
j3 0:d2693f27f344 236 }
j3 0:d2693f27f344 237
j3 0:d2693f27f344 238 //get new duty cycle for left motor
j3 0:d2693f27f344 239 l_duty_cycle = ((KP*current_error) + (KI*integral) + (KD*derivative));
j3 0:d2693f27f344 240 if(l_duty_cycle > MAX_DUTY_CYCLE)
j3 0:d2693f27f344 241 {
j3 0:d2693f27f344 242 l_duty_cycle = MAX_DUTY_CYCLE;
j3 0:d2693f27f344 243 }
j3 0:d2693f27f344 244
j3 0:d2693f27f344 245 if(l_duty_cycle < MIN_DUTY_CYCLE)
j3 0:d2693f27f344 246 {
j3 0:d2693f27f344 247 l_duty_cycle = MIN_DUTY_CYCLE;
j3 0:d2693f27f344 248 }
j3 0:d2693f27f344 249
j3 0:d2693f27f344 250 //ramp motors
j3 0:d2693f27f344 251 do
j3 0:d2693f27f344 252 {
j3 0:d2693f27f344 253 r_duty_cycle_old = ramp(r_duty_cycle, r_duty_cycle_old);
j3 0:d2693f27f344 254 motor_shield.set_pwm_duty_cycle(r_motor_driver, r_duty_cycle_old);
j3 0:d2693f27f344 255
j3 0:d2693f27f344 256 l_duty_cycle_old = ramp(l_duty_cycle, l_duty_cycle_old);
j3 0:d2693f27f344 257 motor_shield.set_pwm_duty_cycle(l_motor_driver, l_duty_cycle_old);
j3 0:d2693f27f344 258 }
j3 0:d2693f27f344 259 while((r_duty_cycle != r_duty_cycle_old) || (l_duty_cycle != l_duty_cycle_old));
j3 0:d2693f27f344 260 }
j3 0:d2693f27f344 261
j3 0:d2693f27f344 262 //shutdown
j3 0:d2693f27f344 263 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::BRAKE);
j3 0:d2693f27f344 264 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::BRAKE);
j3 0:d2693f27f344 265 }
j3 0:d2693f27f344 266 }
j3 0:d2693f27f344 267
j3 0:d2693f27f344 268
j3 0:d2693f27f344 269 float ramp(float val, float val_old)
j3 0:d2693f27f344 270 {
j3 0:d2693f27f344 271 if(val > val_old)
j3 0:d2693f27f344 272 {
j3 0:d2693f27f344 273 if((val - val_old) >= 0.1)
j3 0:d2693f27f344 274 {
j3 0:d2693f27f344 275 val_old += 0.1;
j3 0:d2693f27f344 276 }
j3 0:d2693f27f344 277 else
j3 0:d2693f27f344 278 {
j3 0:d2693f27f344 279 val_old = val;
j3 0:d2693f27f344 280 }
j3 0:d2693f27f344 281 }
j3 0:d2693f27f344 282 else if(val < val_old)
j3 0:d2693f27f344 283 {
j3 0:d2693f27f344 284 if((val_old - val) >= 0.1)
j3 0:d2693f27f344 285 {
j3 0:d2693f27f344 286 val_old -= 0.1;
j3 0:d2693f27f344 287 }
j3 0:d2693f27f344 288 else
j3 0:d2693f27f344 289 {
j3 0:d2693f27f344 290 val_old = val;
j3 0:d2693f27f344 291 }
j3 0:d2693f27f344 292 }
j3 0:d2693f27f344 293 else
j3 0:d2693f27f344 294 {
j3 0:d2693f27f344 295 val_old = val;
j3 0:d2693f27f344 296 }
j3 0:d2693f27f344 297
j3 0:d2693f27f344 298 return(val_old);
j3 0:d2693f27f344 299 }