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?
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 | //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 | } |