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?
User | Revision | Line number | New 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 | } |