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@6:dfbcdc63d4f8, 2016-02-16 (annotated)

Committer:
j3
Date:
Tue Feb 16 22:45:21 2016 +0000
Revision:
6:dfbcdc63d4f8
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 1:b928ca54cd1a 20 //state variables for ISR
j3 4:eb6d6d25355c 21 volatile 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 6:dfbcdc63d4f8 34 //Function for reading QTR-RC infrared sensor from Pololu
j3 6:dfbcdc63d4f8 35 uint8_t get_ir_bus(BusInOut &ir_bus, uint16_t *ir_vals, uint16_t &sample_cnt);
j3 6:dfbcdc63d4f8 36
j3 6:dfbcdc63d4f8 37 //constants used with Pololu sensor
j3 6:dfbcdc63d4f8 38 const uint16_t MAX_SAMPLE_TIME = 1000;
j3 6:dfbcdc63d4f8 39 const uint8_t NUM_SENSORS = 8;
j3 0:d2693f27f344 40
j3 0:d2693f27f344 41 int main(void)
j3 0:d2693f27f344 42 {
j3 4:eb6d6d25355c 43
j3 4:eb6d6d25355c 44 //Trigger interrupt on falling edge of SW3 and cal start_stop fx
j3 0:d2693f27f344 45 start_stop_button.fall(&start_stop);
j3 0:d2693f27f344 46
j3 4:eb6d6d25355c 47 //state indicator, default red for stop
j3 1:b928ca54cd1a 48 start_stop_led = 2; // D7 on D6 off = red
j3 1:b928ca54cd1a 49
j3 4:eb6d6d25355c 50 //Connect IR sensor to port 4
j3 6:dfbcdc63d4f8 51 BusInOut ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
j3 0:d2693f27f344 52
j3 6:dfbcdc63d4f8 53 uint16_t ir_vals[8];
j3 6:dfbcdc63d4f8 54 uint16_t samples = 0;
j3 6:dfbcdc63d4f8 55
j3 6:dfbcdc63d4f8 56 //binary sensor data
j3 4:eb6d6d25355c 57 uint8_t ir_val = 0;
j3 4:eb6d6d25355c 58
j3 4:eb6d6d25355c 59 //used to measure dt
j3 1:b928ca54cd1a 60 DigitalOut loop_pulse(D8, 0);
j3 1:b928ca54cd1a 61
j3 4:eb6d6d25355c 62 //MAXREFDES89# object
j3 0:d2693f27f344 63 Max14871_Shield motor_shield(D14, D15, true);// Default config
j3 5:c673430c8a32 64
j3 5:c673430c8a32 65 // local vars with names that are more meaningful and easier to use than the class name with scope operator
j3 0:d2693f27f344 66 Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4;
j3 0:d2693f27f344 67 Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3;
j3 0:d2693f27f344 68
j3 4:eb6d6d25355c 69 motor_shield.set_current_regulation_mode(r_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
j3 4:eb6d6d25355c 70 motor_shield.set_current_regulation_mode(l_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
j3 4:eb6d6d25355c 71
j3 1:b928ca54cd1a 72 int32_t r_duty_cycle = 0;
j3 1:b928ca54cd1a 73 int32_t l_duty_cycle = 0;
j3 1:b928ca54cd1a 74
j3 5:c673430c8a32 75 const int32_t MAX_DUTY_CYCLE = 80;
j3 5:c673430c8a32 76 const int32_t MIN_DUTY_CYCLE = -80;
j3 0:d2693f27f344 77
j3 6:dfbcdc63d4f8 78 const int32_t TARGET_DUTY_CYCLE = 75; //was 37
j3 4:eb6d6d25355c 79
j3 3:d268f6e06b7a 80
j3 3:d268f6e06b7a 81 /***************************************************
j3 3:d268f6e06b7a 82 | Control Type | KP | KI | KD |
j3 3:d268f6e06b7a 83 |---------------|--------|------------|------------|
j3 3:d268f6e06b7a 84 | P | 0.5Kc | 0 | 0 |
j3 3:d268f6e06b7a 85 |---------------------------------------------------
j3 3:d268f6e06b7a 86 | PI | 0.45Kc | 1.2KpdT/Pc | 0 |
j3 3:d268f6e06b7a 87 |---------------|--------|------------|------------|
j3 3:d268f6e06b7a 88 | PD | 0.80Kc | 0 | KpPc/(8dT) |
j3 3:d268f6e06b7a 89 |---------------|--------|------------|------------|
j3 3:d268f6e06b7a 90 | PID | 0.60Kc | 2KpdT/Pc | KpPc/(8dT) |
j3 3:d268f6e06b7a 91 ***************************************************/
j3 3:d268f6e06b7a 92
j3 3:d268f6e06b7a 93 //Kc = critical gain, gain with just P control at which system becomes marginally stable
j3 3:d268f6e06b7a 94 //Pc = period of oscillation for previous scenario.
j3 3:d268f6e06b7a 95
j3 5:c673430c8a32 96 //Set PID terms to 0 if not used/needed
j3 5:c673430c8a32 97 //For values below Kc = 10 and Pc was measured at 0.33 calculated starting points given in comments below
j3 4:eb6d6d25355c 98 //Ended up decreasing integral term, increasing derivative term and small decrease in proportional term.
j3 6:dfbcdc63d4f8 99 int32_t kp = 5; //was 4
j3 6:dfbcdc63d4f8 100 int32_t ki = 0; //.0576, divide by 1000 later, was 2
j3 6:dfbcdc63d4f8 101 int32_t kd = 0; //156.25, was 250
j3 0:d2693f27f344 102
j3 4:eb6d6d25355c 103 //initialize vars
j3 4:eb6d6d25355c 104 int32_t current_error = 0;
j3 4:eb6d6d25355c 105 int32_t previous_error = current_error;
j3 4:eb6d6d25355c 106 int32_t integral = 0;
j3 4:eb6d6d25355c 107 int32_t derivative = 0;
j3 0:d2693f27f344 108
j3 4:eb6d6d25355c 109 //raw sensor data scaled to a useable range for error signal
j3 4:eb6d6d25355c 110 // SP - feedback raw sensor ir_val
j3 1:b928ca54cd1a 111 const uint8_t ERROR_SIGNAL_P7 = 0xFE; //b11111110
j3 1:b928ca54cd1a 112 const uint8_t ERROR_SIGNAL_P6 = 0xFC; //b11111100
j3 1:b928ca54cd1a 113 const uint8_t ERROR_SIGNAL_P5 = 0xFD; //b11111101
j3 1:b928ca54cd1a 114 const uint8_t ERROR_SIGNAL_P4 = 0xF9; //b11111001
j3 1:b928ca54cd1a 115 const uint8_t ERROR_SIGNAL_P3 = 0xFB; //b11111011
j3 1:b928ca54cd1a 116 const uint8_t ERROR_SIGNAL_P2 = 0xF3; //b11110011
j3 1:b928ca54cd1a 117 const uint8_t ERROR_SIGNAL_P1 = 0xF7; //b11110111
j3 1:b928ca54cd1a 118 const uint8_t ERROR_SIGNAL_00 = 0xE7; //b11100111, feedback = SP
j3 1:b928ca54cd1a 119 const uint8_t ERROR_SIGNAL_N1 = 0xEF; //b11101111
j3 1:b928ca54cd1a 120 const uint8_t ERROR_SIGNAL_N2 = 0xCF; //b11001111
j3 1:b928ca54cd1a 121 const uint8_t ERROR_SIGNAL_N3 = 0xDF; //b11011111
j3 1:b928ca54cd1a 122 const uint8_t ERROR_SIGNAL_N4 = 0x9F; //b10011111
j3 1:b928ca54cd1a 123 const uint8_t ERROR_SIGNAL_N5 = 0xBF; //b10111111
j3 1:b928ca54cd1a 124 const uint8_t ERROR_SIGNAL_N6 = 0x3F; //b00111111
j3 1:b928ca54cd1a 125 const uint8_t ERROR_SIGNAL_N7 = 0x7F; //b01111111
j3 0:d2693f27f344 126
j3 4:eb6d6d25355c 127 #ifdef TUNE_PID
j3 4:eb6d6d25355c 128 char response = 'N';
j3 4:eb6d6d25355c 129
j3 4:eb6d6d25355c 130 printf("\nDo you want to change the PID coefficents? 'Y' or 'N': ");
j3 4:eb6d6d25355c 131 scanf("%c", &response);
j3 4:eb6d6d25355c 132
j3 4:eb6d6d25355c 133 if((response == 'Y') || (response == 'y'))
j3 4:eb6d6d25355c 134 {
j3 4:eb6d6d25355c 135 printf("\nCurrent kp = %d, please enter new kp = ", kp);
j3 4:eb6d6d25355c 136 scanf("%d", &kp);
j3 4:eb6d6d25355c 137
j3 4:eb6d6d25355c 138 printf("\nCurrent ki = %d, please enter new ki = ", ki);
j3 4:eb6d6d25355c 139 scanf("%d", &ki);
j3 4:eb6d6d25355c 140
j3 4:eb6d6d25355c 141 printf("\nCurrent kd = %d, please enter new kd = ", kd);
j3 4:eb6d6d25355c 142 scanf("%d", &kd);
j3 4:eb6d6d25355c 143 }
j3 4:eb6d6d25355c 144
j3 4:eb6d6d25355c 145 printf("\nThe PID coefficents are: ");
j3 4:eb6d6d25355c 146 printf("\nkp = %d", kp);
j3 4:eb6d6d25355c 147 printf("\nki = %d", ki);
j3 4:eb6d6d25355c 148 printf("\nkd = %d\n", kd);
j3 4:eb6d6d25355c 149 #endif//TUNE_PID
j3 1:b928ca54cd1a 150
j3 0:d2693f27f344 151
j3 6:dfbcdc63d4f8 152 //loop time is ~2.86ms
j3 0:d2693f27f344 153 for(;;)
j3 4:eb6d6d25355c 154 {
j3 0:d2693f27f344 155 //wait for start_stop button press
j3 6:dfbcdc63d4f8 156 while(!run)
j3 6:dfbcdc63d4f8 157 {
j3 6:dfbcdc63d4f8 158 //get raw ir sensor data
j3 6:dfbcdc63d4f8 159 ir_val = ~(get_ir_bus(ir_bus, ir_vals, samples));
j3 6:dfbcdc63d4f8 160
j3 6:dfbcdc63d4f8 161 printf("\nir_state = 0x%2x \t samples = %d\n\n", ir_val, samples);
j3 6:dfbcdc63d4f8 162 for(uint8_t idx = 0; idx < 8; idx++)
j3 6:dfbcdc63d4f8 163 {
j3 6:dfbcdc63d4f8 164 printf("%.4d \t", ir_vals[idx]);
j3 6:dfbcdc63d4f8 165 }
j3 6:dfbcdc63d4f8 166 printf("\n\n");
j3 6:dfbcdc63d4f8 167
j3 6:dfbcdc63d4f8 168 wait(1.0);
j3 6:dfbcdc63d4f8 169 }
j3 0:d2693f27f344 170
j3 1:b928ca54cd1a 171 //mode is set to forward, but duty cycle is still 0
j3 0:d2693f27f344 172 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 173 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 174
j3 0:d2693f27f344 175 while(run)
j3 0:d2693f27f344 176 {
j3 4:eb6d6d25355c 177 //measure dt with scope
j3 1:b928ca54cd1a 178 loop_pulse = !loop_pulse;
j3 1:b928ca54cd1a 179
j3 4:eb6d6d25355c 180 //get raw ir sensor data
j3 6:dfbcdc63d4f8 181 ir_val = ~(get_ir_bus(ir_bus, ir_vals, samples));
j3 0:d2693f27f344 182
j3 0:d2693f27f344 183 //scale feedback
j3 0:d2693f27f344 184 switch(ir_val)
j3 0:d2693f27f344 185 {
j3 1:b928ca54cd1a 186 case(ERROR_SIGNAL_P7):
j3 1:b928ca54cd1a 187 current_error = 7;
j3 0:d2693f27f344 188 break;
j3 1:b928ca54cd1a 189
j3 1:b928ca54cd1a 190 case(ERROR_SIGNAL_P6):
j3 1:b928ca54cd1a 191 current_error = 6;
j3 0:d2693f27f344 192 break;
j3 0:d2693f27f344 193
j3 1:b928ca54cd1a 194 case(ERROR_SIGNAL_P5):
j3 1:b928ca54cd1a 195 current_error = 5;
j3 0:d2693f27f344 196 break;
j3 0:d2693f27f344 197
j3 1:b928ca54cd1a 198 case(ERROR_SIGNAL_P4):
j3 1:b928ca54cd1a 199 current_error = 4;
j3 1:b928ca54cd1a 200 break;
j3 1:b928ca54cd1a 201
j3 1:b928ca54cd1a 202 case(ERROR_SIGNAL_P3):
j3 1:b928ca54cd1a 203 current_error = 3;
j3 0:d2693f27f344 204 break;
j3 0:d2693f27f344 205
j3 1:b928ca54cd1a 206 case(ERROR_SIGNAL_P2):
j3 1:b928ca54cd1a 207 current_error = 2;
j3 0:d2693f27f344 208 break;
j3 0:d2693f27f344 209
j3 1:b928ca54cd1a 210 case(ERROR_SIGNAL_P1):
j3 1:b928ca54cd1a 211 current_error = 1;
j3 0:d2693f27f344 212 break;
j3 0:d2693f27f344 213
j3 1:b928ca54cd1a 214 case(ERROR_SIGNAL_00):
j3 1:b928ca54cd1a 215 current_error = 0;
j3 0:d2693f27f344 216 break;
j3 0:d2693f27f344 217
j3 1:b928ca54cd1a 218 case(ERROR_SIGNAL_N1):
j3 1:b928ca54cd1a 219 current_error = -1;
j3 0:d2693f27f344 220 break;
j3 0:d2693f27f344 221
j3 1:b928ca54cd1a 222 case(ERROR_SIGNAL_N2):
j3 1:b928ca54cd1a 223 current_error = -2;
j3 0:d2693f27f344 224 break;
j3 0:d2693f27f344 225
j3 1:b928ca54cd1a 226 case(ERROR_SIGNAL_N3):
j3 1:b928ca54cd1a 227 current_error = -3;
j3 0:d2693f27f344 228 break;
j3 0:d2693f27f344 229
j3 1:b928ca54cd1a 230 case(ERROR_SIGNAL_N4):
j3 1:b928ca54cd1a 231 current_error = -4;
j3 0:d2693f27f344 232 break;
j3 0:d2693f27f344 233
j3 1:b928ca54cd1a 234 case(ERROR_SIGNAL_N5):
j3 1:b928ca54cd1a 235 current_error = -5;
j3 0:d2693f27f344 236 break;
j3 0:d2693f27f344 237
j3 1:b928ca54cd1a 238 case(ERROR_SIGNAL_N6):
j3 1:b928ca54cd1a 239 current_error = -6;
j3 0:d2693f27f344 240 break;
j3 0:d2693f27f344 241
j3 1:b928ca54cd1a 242 case(ERROR_SIGNAL_N7):
j3 1:b928ca54cd1a 243 current_error = -7;
j3 0:d2693f27f344 244 break;
j3 5:c673430c8a32 245
j3 1:b928ca54cd1a 246 default:
j3 3:d268f6e06b7a 247 current_error = previous_error;
j3 1:b928ca54cd1a 248 break;
j3 0:d2693f27f344 249 }
j3 0:d2693f27f344 250
j3 4:eb6d6d25355c 251 /*get integral term, if statement helps w/wind-up
j3 0:d2693f27f344 252
j3 4:eb6d6d25355c 253 from url in file banner
j3 4:eb6d6d25355c 254
j3 4:eb6d6d25355c 255 If integral wind-up is a problem two common solutions are
j3 4:eb6d6d25355c 256 (1) zero the integral, that is set the variable integral
j3 4:eb6d6d25355c 257 equal to zero, every time the error is zero or the
j3 4:eb6d6d25355c 258 error changes sign.
j3 4:eb6d6d25355c 259 (2) "Dampen" the integral by multiplying the accumulated
j3 4:eb6d6d25355c 260 integral by a factor less than one when a new integral
j3 4:eb6d6d25355c 261 is calculated.
j3 4:eb6d6d25355c 262 */
j3 4:eb6d6d25355c 263
j3 0:d2693f27f344 264 if(current_error == 0)
j3 0:d2693f27f344 265 {
j3 0:d2693f27f344 266 integral = 0;
j3 0:d2693f27f344 267 }
j3 0:d2693f27f344 268 else if(((current_error < 0) && (previous_error > 0)) ||
j3 0:d2693f27f344 269 ((current_error > 0) && (previous_error < 0)))
j3 0:d2693f27f344 270 {
j3 0:d2693f27f344 271 integral = 0;
j3 0:d2693f27f344 272 }
j3 0:d2693f27f344 273 else
j3 0:d2693f27f344 274 {
j3 0:d2693f27f344 275 integral = (integral + current_error);
j3 0:d2693f27f344 276 }
j3 0:d2693f27f344 277
j3 0:d2693f27f344 278 //get derivative term
j3 0:d2693f27f344 279 derivative = (current_error - previous_error);
j3 0:d2693f27f344 280
j3 0:d2693f27f344 281 //save current error for next loop
j3 0:d2693f27f344 282 previous_error = current_error;
j3 0:d2693f27f344 283
j3 0:d2693f27f344 284 //get new duty cycle for right motor
j3 6:dfbcdc63d4f8 285 r_duty_cycle = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative));
j3 4:eb6d6d25355c 286
j3 1:b928ca54cd1a 287 //clamp to limits
j3 4:eb6d6d25355c 288 if((r_duty_cycle > MAX_DUTY_CYCLE) || (r_duty_cycle < MIN_DUTY_CYCLE))
j3 4:eb6d6d25355c 289 {
j3 4:eb6d6d25355c 290 if(r_duty_cycle > MAX_DUTY_CYCLE)
j3 4:eb6d6d25355c 291 {
j3 4:eb6d6d25355c 292 r_duty_cycle = MAX_DUTY_CYCLE;
j3 4:eb6d6d25355c 293 }
j3 4:eb6d6d25355c 294 else
j3 4:eb6d6d25355c 295 {
j3 4:eb6d6d25355c 296 r_duty_cycle = MIN_DUTY_CYCLE;
j3 4:eb6d6d25355c 297 }
j3 4:eb6d6d25355c 298 }
j3 1:b928ca54cd1a 299
j3 1:b928ca54cd1a 300
j3 1:b928ca54cd1a 301 //get new duty cycle for left motor
j3 6:dfbcdc63d4f8 302 l_duty_cycle = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative));
j3 4:eb6d6d25355c 303
j3 1:b928ca54cd1a 304 //clamp to limits
j3 4:eb6d6d25355c 305 if((l_duty_cycle > MAX_DUTY_CYCLE) || (l_duty_cycle < MIN_DUTY_CYCLE))
j3 4:eb6d6d25355c 306 {
j3 4:eb6d6d25355c 307 if(l_duty_cycle > MAX_DUTY_CYCLE)
j3 4:eb6d6d25355c 308 {
j3 4:eb6d6d25355c 309 l_duty_cycle = MAX_DUTY_CYCLE;
j3 4:eb6d6d25355c 310 }
j3 4:eb6d6d25355c 311 else
j3 4:eb6d6d25355c 312 {
j3 4:eb6d6d25355c 313 l_duty_cycle = MIN_DUTY_CYCLE;
j3 4:eb6d6d25355c 314 }
j3 4:eb6d6d25355c 315 }
j3 1:b928ca54cd1a 316
j3 1:b928ca54cd1a 317
j3 1:b928ca54cd1a 318 //update direction and duty cycle for right motor
j3 1:b928ca54cd1a 319 if(r_duty_cycle < 0)
j3 0:d2693f27f344 320 {
j3 1:b928ca54cd1a 321 r_duty_cycle = (r_duty_cycle * -1);
j3 1:b928ca54cd1a 322 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::REVERSE);
j3 0:d2693f27f344 323 }
j3 1:b928ca54cd1a 324 else
j3 0:d2693f27f344 325 {
j3 1:b928ca54cd1a 326 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 327 }
j3 5:c673430c8a32 328 motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
j3 0:d2693f27f344 329
j3 1:b928ca54cd1a 330 //update direction and duty cycle for left motor
j3 1:b928ca54cd1a 331 if(l_duty_cycle < 0)
j3 0:d2693f27f344 332 {
j3 1:b928ca54cd1a 333 l_duty_cycle = (l_duty_cycle * -1);
j3 1:b928ca54cd1a 334 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::REVERSE);
j3 0:d2693f27f344 335 }
j3 1:b928ca54cd1a 336 else
j3 0:d2693f27f344 337 {
j3 1:b928ca54cd1a 338 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 1:b928ca54cd1a 339 }
j3 5:c673430c8a32 340 motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
j3 0:d2693f27f344 341 }
j3 0:d2693f27f344 342
j3 0:d2693f27f344 343 //shutdown
j3 1:b928ca54cd1a 344 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 345 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 346
j3 1:b928ca54cd1a 347 //reset all data to initial state
j3 1:b928ca54cd1a 348 r_duty_cycle = 0;
j3 1:b928ca54cd1a 349 l_duty_cycle = 0;
j3 1:b928ca54cd1a 350
j3 1:b928ca54cd1a 351 current_error = 0;
j3 1:b928ca54cd1a 352 previous_error = current_error;
j3 1:b928ca54cd1a 353
j3 1:b928ca54cd1a 354 integral = 0;
j3 1:b928ca54cd1a 355 derivative = 0;
j3 0:d2693f27f344 356 }
j3 0:d2693f27f344 357 }
j3 6:dfbcdc63d4f8 358
j3 6:dfbcdc63d4f8 359
j3 6:dfbcdc63d4f8 360 //Function for reading QTR-RC infrared sensor from Pololu
j3 6:dfbcdc63d4f8 361 uint8_t get_ir_bus(BusInOut &ir_bus, uint16_t *ir_vals, uint16_t &sample_cnt)
j3 6:dfbcdc63d4f8 362 {
j3 6:dfbcdc63d4f8 363 Timer ir_timer;
j3 6:dfbcdc63d4f8 364 uint8_t ir_state = 0;
j3 6:dfbcdc63d4f8 365 uint8_t idx = 0;
j3 6:dfbcdc63d4f8 366 uint16_t sample_time = 0;
j3 6:dfbcdc63d4f8 367
j3 6:dfbcdc63d4f8 368 for(idx = 0; idx < NUM_SENSORS; idx++)
j3 6:dfbcdc63d4f8 369 {
j3 6:dfbcdc63d4f8 370 //set initial val to max for comparison later
j3 6:dfbcdc63d4f8 371 ir_vals[idx] = MAX_SAMPLE_TIME;
j3 6:dfbcdc63d4f8 372
j3 6:dfbcdc63d4f8 373 //build write val for discharging caps based on number of sensors
j3 6:dfbcdc63d4f8 374 ir_state |= (1 << idx);
j3 6:dfbcdc63d4f8 375 }
j3 6:dfbcdc63d4f8 376
j3 6:dfbcdc63d4f8 377 //set bus to output
j3 6:dfbcdc63d4f8 378 ir_bus.output();
j3 6:dfbcdc63d4f8 379 //discharge caps
j3 6:dfbcdc63d4f8 380 ir_bus.write(ir_state);
j3 6:dfbcdc63d4f8 381
j3 6:dfbcdc63d4f8 382 wait_us(10);
j3 6:dfbcdc63d4f8 383
j3 6:dfbcdc63d4f8 384 //set bus to input
j3 6:dfbcdc63d4f8 385 ir_bus.input();
j3 6:dfbcdc63d4f8 386 //ensure no pull-up-down resistors to interfere with reading
j3 6:dfbcdc63d4f8 387 ir_bus.mode(PullNone);
j3 6:dfbcdc63d4f8 388
j3 6:dfbcdc63d4f8 389 //clear sample count and timer
j3 6:dfbcdc63d4f8 390 sample_cnt = 0;
j3 6:dfbcdc63d4f8 391 ir_timer.reset();
j3 6:dfbcdc63d4f8 392 ir_timer.start();
j3 6:dfbcdc63d4f8 393
j3 6:dfbcdc63d4f8 394 do
j3 6:dfbcdc63d4f8 395 {
j3 6:dfbcdc63d4f8 396 sample_cnt++;
j3 6:dfbcdc63d4f8 397 //get sample time from start in micro seconds
j3 6:dfbcdc63d4f8 398 sample_time = ir_timer.read_us();
j3 6:dfbcdc63d4f8 399 //get bus state
j3 6:dfbcdc63d4f8 400 ir_state = ir_bus.read();
j3 6:dfbcdc63d4f8 401
j3 6:dfbcdc63d4f8 402 for(idx = 0; idx < NUM_SENSORS; idx++)
j3 6:dfbcdc63d4f8 403 {
j3 6:dfbcdc63d4f8 404 //if pin state switched to 'low', record sample time
j3 6:dfbcdc63d4f8 405 if(!(ir_state & (1 << idx)) && (ir_vals[idx] > sample_time))
j3 6:dfbcdc63d4f8 406 {
j3 6:dfbcdc63d4f8 407 ir_vals[idx] = sample_time;
j3 6:dfbcdc63d4f8 408 }
j3 6:dfbcdc63d4f8 409 }
j3 6:dfbcdc63d4f8 410 }
j3 6:dfbcdc63d4f8 411 while(sample_time < MAX_SAMPLE_TIME);
j3 6:dfbcdc63d4f8 412
j3 6:dfbcdc63d4f8 413 ir_timer.stop();
j3 6:dfbcdc63d4f8 414
j3 6:dfbcdc63d4f8 415 return(ir_state);
j3 6:dfbcdc63d4f8 416 }