Line following bot using MAXREFDES89, MAX32600MBED, and Pololu QTR-RC sensor
Dependencies: MAX14871_Shield mbed
main.cpp@4:eb6d6d25355c, 2016-01-06 (annotated)
- 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?
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 | 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 | } |