Justin Jordan / Mbed 2 deprecated Line_Following_Bot

Line following bot using MAXREFDES89 and MAX32600MBED

Dependencies:   MAX14871_Shield mbed

main.cpp@6:428538df7b63, 2016-02-16 (annotated)

Committer:
j3
Date:
Tue Feb 16 22:43:52 2016 +0000
Revision:
6:428538df7b63
Parent:
5:c673430c8a32 
init publish

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 4:eb6d6d25355c 12
j3 0:d2693f27f344 13 #include "mbed.h"
j3 0:d2693f27f344 14 #include "max14871_shield.h"
j3 0:d2693f27f344 15
j3 0:d2693f27f344 16
j3 4:eb6d6d25355c 17 //comment out following line for normal operation
j3 4:eb6d6d25355c 18 //#define TUNE_PID 1
j3 0:d2693f27f344 19
j3 6:428538df7b63 20
j3 1:b928ca54cd1a 21 //state variables for ISR
j3 4:eb6d6d25355c 22 volatile bool run = false;
j3 1:b928ca54cd1a 23 BusOut start_stop_led(D6, D7);
j3 0:d2693f27f344 24
j3 1:b928ca54cd1a 25 //Input to trigger interrupt off of
j3 0:d2693f27f344 26 InterruptIn start_stop_button(SW3);
j3 0:d2693f27f344 27
j3 1:b928ca54cd1a 28 //callback fx for ISR
j3 0:d2693f27f344 29 void start_stop()
j3 0:d2693f27f344 30 {
j3 0:d2693f27f344 31 run = !run;
j3 1:b928ca54cd1a 32 start_stop_led = (start_stop_led ^ 3);
j3 0:d2693f27f344 33 }
j3 0:d2693f27f344 34
j3 0:d2693f27f344 35
j3 0:d2693f27f344 36 int main(void)
j3 0:d2693f27f344 37 {
j3 4:eb6d6d25355c 38
j3 4:eb6d6d25355c 39 //Trigger interrupt on falling edge of SW3 and cal start_stop fx
j3 0:d2693f27f344 40 start_stop_button.fall(&start_stop);
j3 0:d2693f27f344 41
j3 4:eb6d6d25355c 42 //state indicator, default red for stop
j3 1:b928ca54cd1a 43 start_stop_led = 2; // D7 on D6 off = red
j3 1:b928ca54cd1a 44
j3 4:eb6d6d25355c 45 //Connect IR sensor to port 4
j3 0:d2693f27f344 46 BusIn ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
j3 6:428538df7b63 47
j3 0:d2693f27f344 48 DigitalOut ir_bus_enable(D3, 1); //active high enable
j3 0:d2693f27f344 49
j3 6:428538df7b63 50 //binary sensor data
j3 4:eb6d6d25355c 51 uint8_t ir_val = 0;
j3 4:eb6d6d25355c 52
j3 4:eb6d6d25355c 53 //used to measure dt
j3 1:b928ca54cd1a 54 DigitalOut loop_pulse(D8, 0);
j3 1:b928ca54cd1a 55
j3 4:eb6d6d25355c 56 //MAXREFDES89# object
j3 0:d2693f27f344 57 Max14871_Shield motor_shield(D14, D15, true);// Default config
j3 5:c673430c8a32 58
j3 5:c673430c8a32 59 // local vars with names that are more meaningful and easier to use than the class name with scope operator
j3 0:d2693f27f344 60 Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4;
j3 0:d2693f27f344 61 Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3;
j3 0:d2693f27f344 62
j3 4:eb6d6d25355c 63 motor_shield.set_current_regulation_mode(r_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
j3 4:eb6d6d25355c 64 motor_shield.set_current_regulation_mode(l_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
j3 4:eb6d6d25355c 65
j3 1:b928ca54cd1a 66 int32_t r_duty_cycle = 0;
j3 1:b928ca54cd1a 67 int32_t l_duty_cycle = 0;
j3 1:b928ca54cd1a 68
j3 5:c673430c8a32 69 const int32_t MAX_DUTY_CYCLE = 80;
j3 5:c673430c8a32 70 const int32_t MIN_DUTY_CYCLE = -80;
j3 0:d2693f27f344 71
j3 5:c673430c8a32 72 const int32_t TARGET_DUTY_CYCLE = 37;
j3 4:eb6d6d25355c 73
j3 3:d268f6e06b7a 74
j3 3:d268f6e06b7a 75 /***************************************************
j3 3:d268f6e06b7a 76 | Control Type | KP | KI | KD |
j3 3:d268f6e06b7a 77 |---------------|--------|------------|------------|
j3 3:d268f6e06b7a 78 | P | 0.5Kc | 0 | 0 |
j3 3:d268f6e06b7a 79 |---------------------------------------------------
j3 3:d268f6e06b7a 80 | PI | 0.45Kc | 1.2KpdT/Pc | 0 |
j3 3:d268f6e06b7a 81 |---------------|--------|------------|------------|
j3 3:d268f6e06b7a 82 | PD | 0.80Kc | 0 | KpPc/(8dT) |
j3 3:d268f6e06b7a 83 |---------------|--------|------------|------------|
j3 3:d268f6e06b7a 84 | PID | 0.60Kc | 2KpdT/Pc | KpPc/(8dT) |
j3 3:d268f6e06b7a 85 ***************************************************/
j3 3:d268f6e06b7a 86
j3 3:d268f6e06b7a 87 //Kc = critical gain, gain with just P control at which system becomes marginally stable
j3 3:d268f6e06b7a 88 //Pc = period of oscillation for previous scenario.
j3 3:d268f6e06b7a 89
j3 5:c673430c8a32 90 //Set PID terms to 0 if not used/needed
j3 5:c673430c8a32 91 //For values below Kc = 10 and Pc was measured at 0.33 calculated starting points given in comments below
j3 4:eb6d6d25355c 92 //Ended up decreasing integral term, increasing derivative term and small decrease in proportional term.
j3 6:428538df7b63 93 int32_t kp = 4; //
j3 6:428538df7b63 94 int32_t ki = 2; //.0576, divide by 1000 later
j3 6:428538df7b63 95 int32_t kd = 250; //156.25
j3 0:d2693f27f344 96
j3 4:eb6d6d25355c 97 //initialize vars
j3 4:eb6d6d25355c 98 int32_t current_error = 0;
j3 4:eb6d6d25355c 99 int32_t previous_error = current_error;
j3 4:eb6d6d25355c 100 int32_t integral = 0;
j3 4:eb6d6d25355c 101 int32_t derivative = 0;
j3 0:d2693f27f344 102
j3 4:eb6d6d25355c 103 //raw sensor data scaled to a useable range for error signal
j3 4:eb6d6d25355c 104 // SP - feedback raw sensor ir_val
j3 1:b928ca54cd1a 105 const uint8_t ERROR_SIGNAL_P7 = 0xFE; //b11111110
j3 1:b928ca54cd1a 106 const uint8_t ERROR_SIGNAL_P6 = 0xFC; //b11111100
j3 1:b928ca54cd1a 107 const uint8_t ERROR_SIGNAL_P5 = 0xFD; //b11111101
j3 1:b928ca54cd1a 108 const uint8_t ERROR_SIGNAL_P4 = 0xF9; //b11111001
j3 1:b928ca54cd1a 109 const uint8_t ERROR_SIGNAL_P3 = 0xFB; //b11111011
j3 1:b928ca54cd1a 110 const uint8_t ERROR_SIGNAL_P2 = 0xF3; //b11110011
j3 1:b928ca54cd1a 111 const uint8_t ERROR_SIGNAL_P1 = 0xF7; //b11110111
j3 1:b928ca54cd1a 112 const uint8_t ERROR_SIGNAL_00 = 0xE7; //b11100111, feedback = SP
j3 1:b928ca54cd1a 113 const uint8_t ERROR_SIGNAL_N1 = 0xEF; //b11101111
j3 1:b928ca54cd1a 114 const uint8_t ERROR_SIGNAL_N2 = 0xCF; //b11001111
j3 1:b928ca54cd1a 115 const uint8_t ERROR_SIGNAL_N3 = 0xDF; //b11011111
j3 1:b928ca54cd1a 116 const uint8_t ERROR_SIGNAL_N4 = 0x9F; //b10011111
j3 1:b928ca54cd1a 117 const uint8_t ERROR_SIGNAL_N5 = 0xBF; //b10111111
j3 1:b928ca54cd1a 118 const uint8_t ERROR_SIGNAL_N6 = 0x3F; //b00111111
j3 1:b928ca54cd1a 119 const uint8_t ERROR_SIGNAL_N7 = 0x7F; //b01111111
j3 0:d2693f27f344 120
j3 4:eb6d6d25355c 121 #ifdef TUNE_PID
j3 4:eb6d6d25355c 122 char response = 'N';
j3 4:eb6d6d25355c 123
j3 4:eb6d6d25355c 124 printf("\nDo you want to change the PID coefficents? 'Y' or 'N': ");
j3 4:eb6d6d25355c 125 scanf("%c", &response);
j3 4:eb6d6d25355c 126
j3 4:eb6d6d25355c 127 if((response == 'Y') || (response == 'y'))
j3 4:eb6d6d25355c 128 {
j3 4:eb6d6d25355c 129 printf("\nCurrent kp = %d, please enter new kp = ", kp);
j3 4:eb6d6d25355c 130 scanf("%d", &kp);
j3 4:eb6d6d25355c 131
j3 4:eb6d6d25355c 132 printf("\nCurrent ki = %d, please enter new ki = ", ki);
j3 4:eb6d6d25355c 133 scanf("%d", &ki);
j3 4:eb6d6d25355c 134
j3 4:eb6d6d25355c 135 printf("\nCurrent kd = %d, please enter new kd = ", kd);
j3 4:eb6d6d25355c 136 scanf("%d", &kd);
j3 4:eb6d6d25355c 137 }
j3 4:eb6d6d25355c 138
j3 4:eb6d6d25355c 139 printf("\nThe PID coefficents are: ");
j3 4:eb6d6d25355c 140 printf("\nkp = %d", kp);
j3 4:eb6d6d25355c 141 printf("\nki = %d", ki);
j3 4:eb6d6d25355c 142 printf("\nkd = %d\n", kd);
j3 4:eb6d6d25355c 143 #endif//TUNE_PID
j3 1:b928ca54cd1a 144
j3 0:d2693f27f344 145
j3 4:eb6d6d25355c 146 //loop time is ~1.6ms
j3 0:d2693f27f344 147 for(;;)
j3 4:eb6d6d25355c 148 {
j3 0:d2693f27f344 149 //wait for start_stop button press
j3 0:d2693f27f344 150 while(!run);
j3 0:d2693f27f344 151
j3 1:b928ca54cd1a 152 //mode is set to forward, but duty cycle is still 0
j3 0:d2693f27f344 153 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 154 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 155
j3 0:d2693f27f344 156 while(run)
j3 0:d2693f27f344 157 {
j3 4:eb6d6d25355c 158 //measure dt with scope
j3 1:b928ca54cd1a 159 loop_pulse = !loop_pulse;
j3 1:b928ca54cd1a 160
j3 4:eb6d6d25355c 161 //get raw ir sensor data
j3 6:428538df7b63 162 ir_val = ~(ir_bus.read()); // use with Parallax Sensor
j3 0:d2693f27f344 163
j3 0:d2693f27f344 164 //scale feedback
j3 0:d2693f27f344 165 switch(ir_val)
j3 0:d2693f27f344 166 {
j3 1:b928ca54cd1a 167 case(ERROR_SIGNAL_P7):
j3 1:b928ca54cd1a 168 current_error = 7;
j3 0:d2693f27f344 169 break;
j3 1:b928ca54cd1a 170
j3 1:b928ca54cd1a 171 case(ERROR_SIGNAL_P6):
j3 1:b928ca54cd1a 172 current_error = 6;
j3 0:d2693f27f344 173 break;
j3 0:d2693f27f344 174
j3 1:b928ca54cd1a 175 case(ERROR_SIGNAL_P5):
j3 1:b928ca54cd1a 176 current_error = 5;
j3 0:d2693f27f344 177 break;
j3 0:d2693f27f344 178
j3 1:b928ca54cd1a 179 case(ERROR_SIGNAL_P4):
j3 1:b928ca54cd1a 180 current_error = 4;
j3 1:b928ca54cd1a 181 break;
j3 1:b928ca54cd1a 182
j3 1:b928ca54cd1a 183 case(ERROR_SIGNAL_P3):
j3 1:b928ca54cd1a 184 current_error = 3;
j3 0:d2693f27f344 185 break;
j3 0:d2693f27f344 186
j3 1:b928ca54cd1a 187 case(ERROR_SIGNAL_P2):
j3 1:b928ca54cd1a 188 current_error = 2;
j3 0:d2693f27f344 189 break;
j3 0:d2693f27f344 190
j3 1:b928ca54cd1a 191 case(ERROR_SIGNAL_P1):
j3 1:b928ca54cd1a 192 current_error = 1;
j3 0:d2693f27f344 193 break;
j3 0:d2693f27f344 194
j3 1:b928ca54cd1a 195 case(ERROR_SIGNAL_00):
j3 1:b928ca54cd1a 196 current_error = 0;
j3 0:d2693f27f344 197 break;
j3 0:d2693f27f344 198
j3 1:b928ca54cd1a 199 case(ERROR_SIGNAL_N1):
j3 1:b928ca54cd1a 200 current_error = -1;
j3 0:d2693f27f344 201 break;
j3 0:d2693f27f344 202
j3 1:b928ca54cd1a 203 case(ERROR_SIGNAL_N2):
j3 1:b928ca54cd1a 204 current_error = -2;
j3 0:d2693f27f344 205 break;
j3 0:d2693f27f344 206
j3 1:b928ca54cd1a 207 case(ERROR_SIGNAL_N3):
j3 1:b928ca54cd1a 208 current_error = -3;
j3 0:d2693f27f344 209 break;
j3 0:d2693f27f344 210
j3 1:b928ca54cd1a 211 case(ERROR_SIGNAL_N4):
j3 1:b928ca54cd1a 212 current_error = -4;
j3 0:d2693f27f344 213 break;
j3 0:d2693f27f344 214
j3 1:b928ca54cd1a 215 case(ERROR_SIGNAL_N5):
j3 1:b928ca54cd1a 216 current_error = -5;
j3 0:d2693f27f344 217 break;
j3 0:d2693f27f344 218
j3 1:b928ca54cd1a 219 case(ERROR_SIGNAL_N6):
j3 1:b928ca54cd1a 220 current_error = -6;
j3 0:d2693f27f344 221 break;
j3 0:d2693f27f344 222
j3 1:b928ca54cd1a 223 case(ERROR_SIGNAL_N7):
j3 1:b928ca54cd1a 224 current_error = -7;
j3 0:d2693f27f344 225 break;
j3 5:c673430c8a32 226
j3 1:b928ca54cd1a 227 default:
j3 3:d268f6e06b7a 228 current_error = previous_error;
j3 1:b928ca54cd1a 229 break;
j3 0:d2693f27f344 230 }
j3 0:d2693f27f344 231
j3 4:eb6d6d25355c 232 /*get integral term, if statement helps w/wind-up
j3 0:d2693f27f344 233
j3 4:eb6d6d25355c 234 from url in file banner
j3 4:eb6d6d25355c 235
j3 4:eb6d6d25355c 236 If integral wind-up is a problem two common solutions are
j3 4:eb6d6d25355c 237 (1) zero the integral, that is set the variable integral
j3 4:eb6d6d25355c 238 equal to zero, every time the error is zero or the
j3 4:eb6d6d25355c 239 error changes sign.
j3 4:eb6d6d25355c 240 (2) "Dampen" the integral by multiplying the accumulated
j3 4:eb6d6d25355c 241 integral by a factor less than one when a new integral
j3 4:eb6d6d25355c 242 is calculated.
j3 4:eb6d6d25355c 243 */
j3 4:eb6d6d25355c 244
j3 0:d2693f27f344 245 if(current_error == 0)
j3 0:d2693f27f344 246 {
j3 0:d2693f27f344 247 integral = 0;
j3 0:d2693f27f344 248 }
j3 0:d2693f27f344 249 else if(((current_error < 0) && (previous_error > 0)) ||
j3 0:d2693f27f344 250 ((current_error > 0) && (previous_error < 0)))
j3 0:d2693f27f344 251 {
j3 0:d2693f27f344 252 integral = 0;
j3 0:d2693f27f344 253 }
j3 0:d2693f27f344 254 else
j3 0:d2693f27f344 255 {
j3 0:d2693f27f344 256 integral = (integral + current_error);
j3 0:d2693f27f344 257 }
j3 0:d2693f27f344 258
j3 0:d2693f27f344 259 //get derivative term
j3 0:d2693f27f344 260 derivative = (current_error - previous_error);
j3 0:d2693f27f344 261
j3 0:d2693f27f344 262 //save current error for next loop
j3 0:d2693f27f344 263 previous_error = current_error;
j3 0:d2693f27f344 264
j3 0:d2693f27f344 265 //get new duty cycle for right motor
j3 4:eb6d6d25355c 266 r_duty_cycle = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative));
j3 4:eb6d6d25355c 267
j3 1:b928ca54cd1a 268 //clamp to limits
j3 4:eb6d6d25355c 269 if((r_duty_cycle > MAX_DUTY_CYCLE) || (r_duty_cycle < MIN_DUTY_CYCLE))
j3 4:eb6d6d25355c 270 {
j3 4:eb6d6d25355c 271 if(r_duty_cycle > MAX_DUTY_CYCLE)
j3 4:eb6d6d25355c 272 {
j3 4:eb6d6d25355c 273 r_duty_cycle = MAX_DUTY_CYCLE;
j3 4:eb6d6d25355c 274 }
j3 4:eb6d6d25355c 275 else
j3 4:eb6d6d25355c 276 {
j3 4:eb6d6d25355c 277 r_duty_cycle = MIN_DUTY_CYCLE;
j3 4:eb6d6d25355c 278 }
j3 4:eb6d6d25355c 279 }
j3 1:b928ca54cd1a 280
j3 1:b928ca54cd1a 281
j3 1:b928ca54cd1a 282 //get new duty cycle for left motor
j3 4:eb6d6d25355c 283 l_duty_cycle = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative));
j3 4:eb6d6d25355c 284
j3 1:b928ca54cd1a 285 //clamp to limits
j3 4:eb6d6d25355c 286 if((l_duty_cycle > MAX_DUTY_CYCLE) || (l_duty_cycle < MIN_DUTY_CYCLE))
j3 4:eb6d6d25355c 287 {
j3 4:eb6d6d25355c 288 if(l_duty_cycle > MAX_DUTY_CYCLE)
j3 4:eb6d6d25355c 289 {
j3 4:eb6d6d25355c 290 l_duty_cycle = MAX_DUTY_CYCLE;
j3 4:eb6d6d25355c 291 }
j3 4:eb6d6d25355c 292 else
j3 4:eb6d6d25355c 293 {
j3 4:eb6d6d25355c 294 l_duty_cycle = MIN_DUTY_CYCLE;
j3 4:eb6d6d25355c 295 }
j3 4:eb6d6d25355c 296 }
j3 1:b928ca54cd1a 297
j3 1:b928ca54cd1a 298
j3 1:b928ca54cd1a 299 //update direction and duty cycle for right motor
j3 1:b928ca54cd1a 300 if(r_duty_cycle < 0)
j3 0:d2693f27f344 301 {
j3 1:b928ca54cd1a 302 r_duty_cycle = (r_duty_cycle * -1);
j3 1:b928ca54cd1a 303 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::REVERSE);
j3 0:d2693f27f344 304 }
j3 1:b928ca54cd1a 305 else
j3 0:d2693f27f344 306 {
j3 1:b928ca54cd1a 307 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 308 }
j3 5:c673430c8a32 309 motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
j3 0:d2693f27f344 310
j3 1:b928ca54cd1a 311 //update direction and duty cycle for left motor
j3 1:b928ca54cd1a 312 if(l_duty_cycle < 0)
j3 0:d2693f27f344 313 {
j3 1:b928ca54cd1a 314 l_duty_cycle = (l_duty_cycle * -1);
j3 1:b928ca54cd1a 315 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::REVERSE);
j3 0:d2693f27f344 316 }
j3 1:b928ca54cd1a 317 else
j3 0:d2693f27f344 318 {
j3 1:b928ca54cd1a 319 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 1:b928ca54cd1a 320 }
j3 5:c673430c8a32 321 motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
j3 0:d2693f27f344 322 }
j3 0:d2693f27f344 323
j3 0:d2693f27f344 324 //shutdown
j3 1:b928ca54cd1a 325 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 326 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 327
j3 1:b928ca54cd1a 328 //reset all data to initial state
j3 1:b928ca54cd1a 329 r_duty_cycle = 0;
j3 1:b928ca54cd1a 330 l_duty_cycle = 0;
j3 1:b928ca54cd1a 331
j3 1:b928ca54cd1a 332 current_error = 0;
j3 1:b928ca54cd1a 333 previous_error = current_error;
j3 1:b928ca54cd1a 334
j3 1:b928ca54cd1a 335 integral = 0;
j3 1:b928ca54cd1a 336 derivative = 0;
j3 0:d2693f27f344 337 }
j3 0:d2693f27f344 338 }