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

Dependencies:   MAX14871_Shield mbed

Committer:
j3
Date:
Wed Jan 06 22:57:35 2016 +0000
Revision:
4:eb6d6d25355c
Parent:
3:d268f6e06b7a
Child:
5:c673430c8a32
Working a lot better with target duty cycle of 35% and tuned loop.  No more wobbles.  Will keep trying to increase duty cycle.

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
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 4:eb6d6d25355c 57
j3 4:eb6d6d25355c 58 //local vars 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 1:b928ca54cd1a 68 const int32_t MAX_DUTY_CYCLE = 100;
j3 1:b928ca54cd1a 69 const int32_t MIN_DUTY_CYCLE = -100;
j3 0:d2693f27f344 70
j3 4:eb6d6d25355c 71 const int32_t TARGET_DUTY_CYCLE = 35;
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 4:eb6d6d25355c 88 //for values below Kc = 10 and Pc was measured at 0.33
j3 3:d268f6e06b7a 89
j3 1:b928ca54cd1a 90 //set PID terms to 0 if not used/needed
j3 4:eb6d6d25355c 91 //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 4:eb6d6d25355c 93 int32_t kp = 5; //6
j3 4:eb6d6d25355c 94 int32_t ki = 10; //.0576, divide by 1000 later
j3 4:eb6d6d25355c 95 int32_t kd = 500; //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 1:b928ca54cd1a 121 //special case error signals, 90 degree turns
j3 1:b928ca54cd1a 122 const uint8_t ERROR_LT0 = 0xE0; //b11100000
j3 1:b928ca54cd1a 123 const uint8_t ERROR_LT1 = 0xF0; //b11110000
j3 1:b928ca54cd1a 124 const uint8_t ERROR_LT2 = 0xF8; //b11111000
j3 1:b928ca54cd1a 125 const uint8_t ERROR_RT0 = 0x07; //b00000111
j3 1:b928ca54cd1a 126 const uint8_t ERROR_RT1 = 0x0F; //b00001111
j3 1:b928ca54cd1a 127 const uint8_t ERROR_RT2 = 0x1F; //b00011111
j3 1:b928ca54cd1a 128
j3 3:d268f6e06b7a 129 //Lost bot error signal
j3 3:d268f6e06b7a 130 const uint8_t ERROR_OFF_TRACK = 0xFF; //b11111111
j3 3:d268f6e06b7a 131
j3 1:b928ca54cd1a 132
j3 4:eb6d6d25355c 133 #ifdef TUNE_PID
j3 4:eb6d6d25355c 134 char response = 'N';
j3 4:eb6d6d25355c 135
j3 4:eb6d6d25355c 136 printf("\nDo you want to change the PID coefficents? 'Y' or 'N': ");
j3 4:eb6d6d25355c 137 scanf("%c", &response);
j3 4:eb6d6d25355c 138
j3 4:eb6d6d25355c 139 if((response == 'Y') || (response == 'y'))
j3 4:eb6d6d25355c 140 {
j3 4:eb6d6d25355c 141 printf("\nCurrent kp = %d, please enter new kp = ", kp);
j3 4:eb6d6d25355c 142 scanf("%d", &kp);
j3 4:eb6d6d25355c 143
j3 4:eb6d6d25355c 144 printf("\nCurrent ki = %d, please enter new ki = ", ki);
j3 4:eb6d6d25355c 145 scanf("%d", &ki);
j3 4:eb6d6d25355c 146
j3 4:eb6d6d25355c 147 printf("\nCurrent kd = %d, please enter new kd = ", kd);
j3 4:eb6d6d25355c 148 scanf("%d", &kd);
j3 4:eb6d6d25355c 149 }
j3 4:eb6d6d25355c 150
j3 4:eb6d6d25355c 151 printf("\nThe PID coefficents are: ");
j3 4:eb6d6d25355c 152 printf("\nkp = %d", kp);
j3 4:eb6d6d25355c 153 printf("\nki = %d", ki);
j3 4:eb6d6d25355c 154 printf("\nkd = %d\n", kd);
j3 4:eb6d6d25355c 155 #endif//TUNE_PID
j3 1:b928ca54cd1a 156
j3 0:d2693f27f344 157
j3 4:eb6d6d25355c 158 //loop time is ~1.6ms
j3 0:d2693f27f344 159 for(;;)
j3 4:eb6d6d25355c 160 {
j3 0:d2693f27f344 161 //wait for start_stop button press
j3 0:d2693f27f344 162 while(!run);
j3 0:d2693f27f344 163
j3 1:b928ca54cd1a 164 //mode is set to forward, but duty cycle is still 0
j3 0:d2693f27f344 165 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 166 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 0:d2693f27f344 167
j3 0:d2693f27f344 168 while(run)
j3 0:d2693f27f344 169 {
j3 4:eb6d6d25355c 170 //measure dt with scope
j3 1:b928ca54cd1a 171 loop_pulse = !loop_pulse;
j3 1:b928ca54cd1a 172
j3 4:eb6d6d25355c 173 //get raw ir sensor data
j3 0:d2693f27f344 174 ir_val = ~(ir_bus.read());
j3 0:d2693f27f344 175
j3 0:d2693f27f344 176 //scale feedback
j3 0:d2693f27f344 177 switch(ir_val)
j3 0:d2693f27f344 178 {
j3 1:b928ca54cd1a 179 case(ERROR_SIGNAL_P7):
j3 1:b928ca54cd1a 180 current_error = 7;
j3 0:d2693f27f344 181 break;
j3 1:b928ca54cd1a 182
j3 1:b928ca54cd1a 183 case(ERROR_SIGNAL_P6):
j3 1:b928ca54cd1a 184 current_error = 6;
j3 0:d2693f27f344 185 break;
j3 0:d2693f27f344 186
j3 1:b928ca54cd1a 187 case(ERROR_SIGNAL_P5):
j3 1:b928ca54cd1a 188 current_error = 5;
j3 0:d2693f27f344 189 break;
j3 0:d2693f27f344 190
j3 1:b928ca54cd1a 191 case(ERROR_SIGNAL_P4):
j3 1:b928ca54cd1a 192 current_error = 4;
j3 1:b928ca54cd1a 193 break;
j3 1:b928ca54cd1a 194
j3 1:b928ca54cd1a 195 case(ERROR_SIGNAL_P3):
j3 1:b928ca54cd1a 196 current_error = 3;
j3 0:d2693f27f344 197 break;
j3 0:d2693f27f344 198
j3 1:b928ca54cd1a 199 case(ERROR_SIGNAL_P2):
j3 1:b928ca54cd1a 200 current_error = 2;
j3 0:d2693f27f344 201 break;
j3 0:d2693f27f344 202
j3 1:b928ca54cd1a 203 case(ERROR_SIGNAL_P1):
j3 1:b928ca54cd1a 204 current_error = 1;
j3 0:d2693f27f344 205 break;
j3 0:d2693f27f344 206
j3 1:b928ca54cd1a 207 case(ERROR_SIGNAL_00):
j3 1:b928ca54cd1a 208 current_error = 0;
j3 0:d2693f27f344 209 break;
j3 0:d2693f27f344 210
j3 1:b928ca54cd1a 211 case(ERROR_SIGNAL_N1):
j3 1:b928ca54cd1a 212 current_error = -1;
j3 0:d2693f27f344 213 break;
j3 0:d2693f27f344 214
j3 1:b928ca54cd1a 215 case(ERROR_SIGNAL_N2):
j3 1:b928ca54cd1a 216 current_error = -2;
j3 0:d2693f27f344 217 break;
j3 0:d2693f27f344 218
j3 1:b928ca54cd1a 219 case(ERROR_SIGNAL_N3):
j3 1:b928ca54cd1a 220 current_error = -3;
j3 0:d2693f27f344 221 break;
j3 0:d2693f27f344 222
j3 1:b928ca54cd1a 223 case(ERROR_SIGNAL_N4):
j3 1:b928ca54cd1a 224 current_error = -4;
j3 0:d2693f27f344 225 break;
j3 0:d2693f27f344 226
j3 1:b928ca54cd1a 227 case(ERROR_SIGNAL_N5):
j3 1:b928ca54cd1a 228 current_error = -5;
j3 0:d2693f27f344 229 break;
j3 0:d2693f27f344 230
j3 1:b928ca54cd1a 231 case(ERROR_SIGNAL_N6):
j3 1:b928ca54cd1a 232 current_error = -6;
j3 0:d2693f27f344 233 break;
j3 0:d2693f27f344 234
j3 1:b928ca54cd1a 235 case(ERROR_SIGNAL_N7):
j3 1:b928ca54cd1a 236 current_error = -7;
j3 0:d2693f27f344 237 break;
j3 4:eb6d6d25355c 238
j3 1:b928ca54cd1a 239 case(ERROR_LT0):
j3 1:b928ca54cd1a 240 current_error = 7;
j3 0:d2693f27f344 241 break;
j3 0:d2693f27f344 242
j3 1:b928ca54cd1a 243 case(ERROR_LT1):
j3 1:b928ca54cd1a 244 current_error = 7;
j3 0:d2693f27f344 245 break;
j3 0:d2693f27f344 246
j3 1:b928ca54cd1a 247 case(ERROR_LT2):
j3 1:b928ca54cd1a 248 current_error = 7;
j3 0:d2693f27f344 249 break;
j3 1:b928ca54cd1a 250
j3 1:b928ca54cd1a 251 case(ERROR_RT0):
j3 1:b928ca54cd1a 252 current_error = -7;
j3 0:d2693f27f344 253 break;
j3 0:d2693f27f344 254
j3 1:b928ca54cd1a 255 case(ERROR_RT1):
j3 1:b928ca54cd1a 256 current_error = -7;
j3 0:d2693f27f344 257 break;
j3 0:d2693f27f344 258
j3 1:b928ca54cd1a 259 case(ERROR_RT2):
j3 1:b928ca54cd1a 260 current_error = -7;
j3 0:d2693f27f344 261 break;
j3 3:d268f6e06b7a 262
j3 3:d268f6e06b7a 263 case(ERROR_OFF_TRACK):
j3 4:eb6d6d25355c 264 //requires reset
j3 4:eb6d6d25355c 265 motor_shield.set_pwm_duty_cycle(r_motor_driver, 0.0f);
j3 4:eb6d6d25355c 266 motor_shield.set_pwm_duty_cycle(l_motor_driver, 0.0f);
j3 4:eb6d6d25355c 267 while(1);
j3 1:b928ca54cd1a 268 default:
j3 3:d268f6e06b7a 269 current_error = previous_error;
j3 1:b928ca54cd1a 270 break;
j3 0:d2693f27f344 271 }
j3 0:d2693f27f344 272
j3 4:eb6d6d25355c 273 /*get integral term, if statement helps w/wind-up
j3 0:d2693f27f344 274
j3 4:eb6d6d25355c 275 from url in file banner
j3 4:eb6d6d25355c 276
j3 4:eb6d6d25355c 277 If integral wind-up is a problem two common solutions are
j3 4:eb6d6d25355c 278 (1) zero the integral, that is set the variable integral
j3 4:eb6d6d25355c 279 equal to zero, every time the error is zero or the
j3 4:eb6d6d25355c 280 error changes sign.
j3 4:eb6d6d25355c 281 (2) "Dampen" the integral by multiplying the accumulated
j3 4:eb6d6d25355c 282 integral by a factor less than one when a new integral
j3 4:eb6d6d25355c 283 is calculated.
j3 4:eb6d6d25355c 284 */
j3 4:eb6d6d25355c 285
j3 0:d2693f27f344 286 if(current_error == 0)
j3 0:d2693f27f344 287 {
j3 0:d2693f27f344 288 integral = 0;
j3 0:d2693f27f344 289 }
j3 0:d2693f27f344 290 else if(((current_error < 0) && (previous_error > 0)) ||
j3 0:d2693f27f344 291 ((current_error > 0) && (previous_error < 0)))
j3 0:d2693f27f344 292 {
j3 0:d2693f27f344 293 integral = 0;
j3 0:d2693f27f344 294 }
j3 0:d2693f27f344 295 else
j3 0:d2693f27f344 296 {
j3 0:d2693f27f344 297 integral = (integral + current_error);
j3 0:d2693f27f344 298 }
j3 0:d2693f27f344 299
j3 0:d2693f27f344 300 //get derivative term
j3 0:d2693f27f344 301 derivative = (current_error - previous_error);
j3 0:d2693f27f344 302
j3 0:d2693f27f344 303 //save current error for next loop
j3 0:d2693f27f344 304 previous_error = current_error;
j3 0:d2693f27f344 305
j3 0:d2693f27f344 306 //get new duty cycle for right motor
j3 4:eb6d6d25355c 307 r_duty_cycle = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative));
j3 4:eb6d6d25355c 308
j3 1:b928ca54cd1a 309 //clamp to limits
j3 4:eb6d6d25355c 310 if((r_duty_cycle > MAX_DUTY_CYCLE) || (r_duty_cycle < MIN_DUTY_CYCLE))
j3 4:eb6d6d25355c 311 {
j3 4:eb6d6d25355c 312 if(r_duty_cycle > MAX_DUTY_CYCLE)
j3 4:eb6d6d25355c 313 {
j3 4:eb6d6d25355c 314 r_duty_cycle = MAX_DUTY_CYCLE;
j3 4:eb6d6d25355c 315 }
j3 4:eb6d6d25355c 316 else
j3 4:eb6d6d25355c 317 {
j3 4:eb6d6d25355c 318 r_duty_cycle = MIN_DUTY_CYCLE;
j3 4:eb6d6d25355c 319 }
j3 4:eb6d6d25355c 320 }
j3 1:b928ca54cd1a 321
j3 1:b928ca54cd1a 322
j3 1:b928ca54cd1a 323 //get new duty cycle for left motor
j3 4:eb6d6d25355c 324 l_duty_cycle = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative));
j3 4:eb6d6d25355c 325
j3 1:b928ca54cd1a 326 //clamp to limits
j3 4:eb6d6d25355c 327 if((l_duty_cycle > MAX_DUTY_CYCLE) || (l_duty_cycle < MIN_DUTY_CYCLE))
j3 4:eb6d6d25355c 328 {
j3 4:eb6d6d25355c 329 if(l_duty_cycle > MAX_DUTY_CYCLE)
j3 4:eb6d6d25355c 330 {
j3 4:eb6d6d25355c 331 l_duty_cycle = MAX_DUTY_CYCLE;
j3 4:eb6d6d25355c 332 }
j3 4:eb6d6d25355c 333 else
j3 4:eb6d6d25355c 334 {
j3 4:eb6d6d25355c 335 l_duty_cycle = MIN_DUTY_CYCLE;
j3 4:eb6d6d25355c 336 }
j3 4:eb6d6d25355c 337 }
j3 1:b928ca54cd1a 338
j3 1:b928ca54cd1a 339
j3 1:b928ca54cd1a 340 //update direction and duty cycle for right motor
j3 1:b928ca54cd1a 341 if(r_duty_cycle < 0)
j3 0:d2693f27f344 342 {
j3 1:b928ca54cd1a 343 r_duty_cycle = (r_duty_cycle * -1);
j3 1:b928ca54cd1a 344
j3 1:b928ca54cd1a 345 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::REVERSE);
j3 1:b928ca54cd1a 346 motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
j3 0:d2693f27f344 347 }
j3 1:b928ca54cd1a 348 else
j3 0:d2693f27f344 349 {
j3 1:b928ca54cd1a 350 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
j3 1:b928ca54cd1a 351 motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
j3 0:d2693f27f344 352 }
j3 0:d2693f27f344 353
j3 1:b928ca54cd1a 354 //update direction and duty cycle for left motor
j3 1:b928ca54cd1a 355 if(l_duty_cycle < 0)
j3 0:d2693f27f344 356 {
j3 1:b928ca54cd1a 357 l_duty_cycle = (l_duty_cycle * -1);
j3 1:b928ca54cd1a 358
j3 1:b928ca54cd1a 359 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::REVERSE);
j3 1:b928ca54cd1a 360 motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
j3 0:d2693f27f344 361 }
j3 1:b928ca54cd1a 362 else
j3 0:d2693f27f344 363 {
j3 1:b928ca54cd1a 364 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
j3 1:b928ca54cd1a 365 motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
j3 1:b928ca54cd1a 366 }
j3 0:d2693f27f344 367 }
j3 0:d2693f27f344 368
j3 0:d2693f27f344 369 //shutdown
j3 1:b928ca54cd1a 370 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 371 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::COAST);
j3 1:b928ca54cd1a 372
j3 1:b928ca54cd1a 373 //reset all data to initial state
j3 1:b928ca54cd1a 374 r_duty_cycle = 0;
j3 1:b928ca54cd1a 375 l_duty_cycle = 0;
j3 1:b928ca54cd1a 376
j3 1:b928ca54cd1a 377 current_error = 0;
j3 1:b928ca54cd1a 378 previous_error = current_error;
j3 1:b928ca54cd1a 379
j3 1:b928ca54cd1a 380 integral = 0;
j3 1:b928ca54cd1a 381 derivative = 0;
j3 0:d2693f27f344 382 }
j3 0:d2693f27f344 383 }