Line following bot using MAXREFDES89 and MAX32600MBED
Dependencies: MAX14871_Shield mbed
main.cpp
- Committer:
- j3
- Date:
- 2015-12-21
- Revision:
- 3:d268f6e06b7a
- Parent:
- 2:2c35ad38bf00
- Child:
- 4:eb6d6d25355c
File content as of revision 3:d268f6e06b7a:
/********************************************************************** * Simple line following bot with PID to demo MAXREFDES89# * PID feedback comes from infrared led sensor from Parallax * https://www.parallax.com/product/28034 * * The following link is a good example for a PID line follower * * http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html * **********************************************************************/ #include "mbed.h" #include "max14871_shield.h" //helper functions to make code in main loop clearer/readable int32_t clamp_duty_cycle(int32_t max, int32_t min, int32_t dc); //state variables for ISR bool run = false; BusOut start_stop_led(D6, D7); //Input to trigger interrupt off of InterruptIn start_stop_button(SW3); //callback fx for ISR void start_stop() { run = !run; start_stop_led = (start_stop_led ^ 3); } int main(void) { start_stop_button.fall(&start_stop); start_stop_led = 2; // D7 on D6 off = red BusIn ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7); DigitalOut ir_bus_enable(D3, 1); //active high enable DigitalOut loop_pulse(D8, 0); Max14871_Shield motor_shield(D14, D15, true);// Default config Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4; Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3; int32_t r_duty_cycle = 0; int32_t l_duty_cycle = 0; const int32_t MAX_DUTY_CYCLE = 100; const int32_t MIN_DUTY_CYCLE = -100; /*************************************************** | Control Type | KP | KI | KD | |---------------|--------|------------|------------| | P | 0.5Kc | 0 | 0 | |--------------------------------------------------- | PI | 0.45Kc | 1.2KpdT/Pc | 0 | |---------------|--------|------------|------------| | PD | 0.80Kc | 0 | KpPc/(8dT) | |---------------|--------|------------|------------| | PID | 0.60Kc | 2KpdT/Pc | KpPc/(8dT) | ***************************************************/ //Kc = critical gain, gain with just P control at which system becomes marginally stable //Pc = period of oscillation for previous scenario. //set PID terms to 0 if not used/needed const int32_t KP = 16; const int32_t KI = 0; const int32_t KD = 0; const int32_t TARGET_DUTY_CYCLE = 50; //raw sensor data uint8_t ir_val = 0; //raw sensor data scaled //to a useable range for //error signal, SP - feedback raw sensor ir_val const uint8_t ERROR_SIGNAL_P7 = 0xFE; //b11111110 const uint8_t ERROR_SIGNAL_P6 = 0xFC; //b11111100 const uint8_t ERROR_SIGNAL_P5 = 0xFD; //b11111101 const uint8_t ERROR_SIGNAL_P4 = 0xF9; //b11111001 const uint8_t ERROR_SIGNAL_P3 = 0xFB; //b11111011 const uint8_t ERROR_SIGNAL_P2 = 0xF3; //b11110011 const uint8_t ERROR_SIGNAL_P1 = 0xF7; //b11110111 const uint8_t ERROR_SIGNAL_00 = 0xE7; //b11100111, feedback = SP const uint8_t ERROR_SIGNAL_N1 = 0xEF; //b11101111 const uint8_t ERROR_SIGNAL_N2 = 0xCF; //b11001111 const uint8_t ERROR_SIGNAL_N3 = 0xDF; //b11011111 const uint8_t ERROR_SIGNAL_N4 = 0x9F; //b10011111 const uint8_t ERROR_SIGNAL_N5 = 0xBF; //b10111111 const uint8_t ERROR_SIGNAL_N6 = 0x3F; //b00111111 const uint8_t ERROR_SIGNAL_N7 = 0x7F; //b01111111 //special case error signals, 90 degree turns const uint8_t ERROR_LT0 = 0xE0; //b11100000 const uint8_t ERROR_LT1 = 0xF0; //b11110000 const uint8_t ERROR_LT2 = 0xF8; //b11111000 const uint8_t ERROR_RT0 = 0x07; //b00000111 const uint8_t ERROR_RT1 = 0x0F; //b00001111 const uint8_t ERROR_RT2 = 0x1F; //b00011111 //Lost bot error signal const uint8_t ERROR_OFF_TRACK = 0xFF; //b11111111 int32_t current_error = 0; int32_t previous_error = 0; int32_t integral = 0; int32_t derivative = 0; for(;;) { //wait for start_stop button press while(!run); //mode is set to forward, but duty cycle is still 0 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD); motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD); while(run) { loop_pulse = !loop_pulse; ir_val = ~(ir_bus.read()); //scale feedback switch(ir_val) { case(ERROR_SIGNAL_P7): current_error = 7; break; case(ERROR_SIGNAL_P6): current_error = 6; break; case(ERROR_SIGNAL_P5): current_error = 5; break; case(ERROR_SIGNAL_P4): current_error = 4; break; case(ERROR_SIGNAL_P3): current_error = 3; break; case(ERROR_SIGNAL_P2): current_error = 2; break; case(ERROR_SIGNAL_P1): current_error = 1; break; case(ERROR_SIGNAL_00): current_error = 0; break; case(ERROR_SIGNAL_N1): current_error = -1; break; case(ERROR_SIGNAL_N2): current_error = -2; break; case(ERROR_SIGNAL_N3): current_error = -3; break; case(ERROR_SIGNAL_N4): current_error = -4; break; case(ERROR_SIGNAL_N5): current_error = -5; break; case(ERROR_SIGNAL_N6): current_error = -6; break; case(ERROR_SIGNAL_N7): current_error = -7; break; case(ERROR_LT0): current_error = 7; break; case(ERROR_LT1): current_error = 7; break; case(ERROR_LT2): current_error = 7; break; case(ERROR_RT0): current_error = -7; break; case(ERROR_RT1): current_error = -7; break; case(ERROR_RT2): current_error = -7; break; case(ERROR_OFF_TRACK): //do circles cause I am lost current_error = 7; break; default: current_error = previous_error; break; } //get integral term, if statement helps w/wind-up if(current_error == 0) { integral = 0; } else if(((current_error < 0) && (previous_error > 0)) || ((current_error > 0) && (previous_error < 0))) { integral = 0; } else { integral = (integral + current_error); } //get derivative term derivative = (current_error - previous_error); //save current error for next loop previous_error = current_error; //get new duty cycle for right motor r_duty_cycle = (TARGET_DUTY_CYCLE - (KP*current_error + (KI*integral) + KD*derivative)); //clamp to limits r_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, r_duty_cycle ); //get new duty cycle for left motor l_duty_cycle = (TARGET_DUTY_CYCLE + (KP*current_error + (KI*integral) + KD*derivative)); //clamp to limits l_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, l_duty_cycle ); //update direction and duty cycle for right motor if(r_duty_cycle < 0) { r_duty_cycle = (r_duty_cycle * -1); motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::REVERSE); motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f); } else { motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD); motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f); } //update direction and duty cycle for left motor if(l_duty_cycle < 0) { l_duty_cycle = (l_duty_cycle * -1); motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::REVERSE); motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f); } else { motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD); motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f); } } //shutdown motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::COAST); motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::COAST); //reset all data to initial state r_duty_cycle = 0; l_duty_cycle = 0; current_error = 0; previous_error = current_error; integral = 0; derivative = 0; } } int32_t clamp_duty_cycle(int32_t max, int32_t min, int32_t dc) { if(dc > max) { dc = max; } if(dc < min) { dc = min; } return(dc); }