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@5:c673430c8a32, 2016-01-10 (annotated)

Committer:
j3
Date:
Sun Jan 10 19:12:08 2016 +0000
Revision:
5:c673430c8a32
Parent:
4:eb6d6d25355c
Child:
6:dfbcdc63d4f8
Target dc = 37, kp = 4, ki = 3, kd = 250.; ; works pretty good, still a little overshoot through the s turn

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