Line following bot using MAXREFDES89 and MAX32600MBED
Dependencies: MAX14871_Shield mbed
Diff: main.cpp
- Revision:
- 1:b928ca54cd1a
- Parent:
- 0:d2693f27f344
- Child:
- 2:2c35ad38bf00
--- a/main.cpp Mon Dec 14 00:03:54 2015 +0000 +++ b/main.cpp Thu Dec 17 02:29:28 2015 +0000 @@ -2,25 +2,33 @@ * 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" -float ramp(float val, float val_old); +//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; -DigitalOut run_led(LED_GREEN, 1); //leds have inverted logic -DigitalOut stop_led(LED_RED, 0); +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; - run_led = !run_led; - stop_led = !stop_led; + start_stop_led = (start_stop_led ^ 3); } @@ -28,179 +36,175 @@ { 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; - const float MAX_DUTY_CYCLE = 1.0; - const float MIN_DUTY_CYCLE = 0.0; + 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; - //set point is for position relative to line, not duty cycle of pwm - const float SP = 0.0; - const float KP = 1.0; - const float KI = 0.0; - const float KD = 0.0; - //const float OFFSET = 0; + //set PID terms to 0 if not used/needed + const int32_t KP = 7; + const int32_t KI = 0; + const int32_t KD = 0; + const int32_t TARGET_DUTY_CYCLE = 50; //starts bot off at 80% duty cycle //raw sensor data uint8_t ir_val = 0; - const uint8_t ZONE_8 = 0xFF; - const uint8_t ZONE_7 = 0xFE; - const uint8_t ZONE_6 = 0xFC; - const uint8_t ZONE_5 = 0xFD; - const uint8_t ZONE_4 = 0xF9; - const uint8_t ZONE_3 = 0xFB; - const uint8_t ZONE_2 = 0xF3; - const uint8_t ZONE_1 = 0xF7; - const uint8_t ZONE_0 = 0xE7; - const uint8_t ZONE_N1 = 0xEF; - const uint8_t ZONE_N2 = 0xCF; - const uint8_t ZONE_N3 = 0xDF; - const uint8_t ZONE_N4 = 0x9F; - const uint8_t ZONE_N5 = 0xBF; - const uint8_t ZONE_N6 = 0x3F; - const uint8_t ZONE_N7 = 0x7F; - const uint8_t ZONE_LT0 = 0xE0; - const uint8_t ZONE_LT1 = 0xF0; - const uint8_t ZONE_LT2 = 0xF8; - const uint8_t ZONE_RT0 = 0x07; - const uint8_t ZONE_RT1 = 0x0F; - const uint8_t ZONE_RT2 = 0x1F; + //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 - float r_duty_cycle = 0.0; - float r_duty_cycle_old = r_duty_cycle; - - float l_duty_cycle = 0.0; - float l_duty_cycle_old = l_duty_cycle; - - float fb = 0; - float current_error = 0; - float previous_error = 0; - float integral = 0; - float derivative = 0; + //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 + + 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.0 + //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(ZONE_8): - if(fb < 0) - { - fb = -8; - } - else - { - fb = 8; - } + case(ERROR_SIGNAL_P7): + current_error = 7; break; - - case(ZONE_7): - fb = 7; + + case(ERROR_SIGNAL_P6): + current_error = 6; break; - case(ZONE_6): - fb = 6; + case(ERROR_SIGNAL_P5): + current_error = 5; break; - case(ZONE_5): - fb = 5; + case(ERROR_SIGNAL_P4): + current_error = 4; + break; + + case(ERROR_SIGNAL_P3): + current_error = 3; break; - case(ZONE_4): - fb = 4; + case(ERROR_SIGNAL_P2): + current_error = 2; break; - case(ZONE_3): - fb = 3; - break; - - case(ZONE_2): - fb = 2; + case(ERROR_SIGNAL_P1): + current_error = 1; break; - case(ZONE_1): - fb = 1; + case(ERROR_SIGNAL_00): + current_error = 0; break; - case(ZONE_0): - fb = 0; + case(ERROR_SIGNAL_N1): + current_error = -1; break; - case(ZONE_N1): - fb = -1; + case(ERROR_SIGNAL_N2): + current_error = -2; break; - case(ZONE_N2): - fb = -2; + case(ERROR_SIGNAL_N3): + current_error = -3; break; - case(ZONE_N3): - fb = -3; + case(ERROR_SIGNAL_N4): + current_error = -4; break; - case(ZONE_N4): - fb = -4; + case(ERROR_SIGNAL_N5): + current_error = -5; break; - case(ZONE_N5): - fb = -5; + case(ERROR_SIGNAL_N6): + current_error = -6; break; - case(ZONE_N6): - fb = -6; + case(ERROR_SIGNAL_N7): + current_error = -7; break; - - case(ZONE_N7): - fb = -7; - break; - - case(ZONE_LT0): - fb = 8; + + case(ERROR_LT0): + current_error = 7; break; - case(ZONE_LT1): - fb = 8; + case(ERROR_LT1): + current_error = 7; break; - case(ZONE_LT2): - fb = 8; + case(ERROR_LT2): + current_error = 7; break; - - case(ZONE_RT0): - fb = -8; + + case(ERROR_RT0): + current_error = -7; break; - case(ZONE_RT1): - fb = -8; + case(ERROR_RT1): + current_error = -7; break; - case(ZONE_RT2): - fb = -8; + case(ERROR_RT2): + current_error = -7; break; - - default: - fb = fb; + + default: + //do circles because I am a lost bot + current_error = 7; + break; } - //get error signal - current_error = SP-fb; //get integral term, if statement helps w/wind-up if(current_error == 0) @@ -224,76 +228,74 @@ previous_error = current_error; //get new duty cycle for right motor - r_duty_cycle = ((KP*current_error) + (KI*integral) + (KD*derivative)); - if(r_duty_cycle > MAX_DUTY_CYCLE) + 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 = MAX_DUTY_CYCLE; + 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); } - - if(r_duty_cycle < MIN_DUTY_CYCLE) + else { - r_duty_cycle = MIN_DUTY_CYCLE; + 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); } - //get new duty cycle for left motor - l_duty_cycle = ((KP*current_error) + (KI*integral) + (KD*derivative)); - if(l_duty_cycle > MAX_DUTY_CYCLE) + //update direction and duty cycle for left motor + if(l_duty_cycle < 0) { - l_duty_cycle = MAX_DUTY_CYCLE; - } - - if(l_duty_cycle < MIN_DUTY_CYCLE) - { - l_duty_cycle = MIN_DUTY_CYCLE; + 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); } - - //ramp motors - do + else { - r_duty_cycle_old = ramp(r_duty_cycle, r_duty_cycle_old); - motor_shield.set_pwm_duty_cycle(r_motor_driver, r_duty_cycle_old); - - l_duty_cycle_old = ramp(l_duty_cycle, l_duty_cycle_old); - motor_shield.set_pwm_duty_cycle(l_motor_driver, l_duty_cycle_old); - } - while((r_duty_cycle != r_duty_cycle_old) || (l_duty_cycle != l_duty_cycle_old)); + 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::BRAKE); - motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::BRAKE); + 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; } } -float ramp(float val, float val_old) +int32_t clamp_duty_cycle(int32_t max, int32_t min, int32_t dc) { - if(val > val_old) - { - if((val - val_old) >= 0.1) - { - val_old += 0.1; - } - else + if(dc > max) { - val_old = val; - } - } - else if(val < val_old) - { - if((val_old - val) >= 0.1) - { - val_old -= 0.1; - } - else + dc = max; + } + + if(dc < min) { - val_old = val; - } - } - else - { - val_old = val; - } - - return(val_old); -} + dc = min; + } + + return(dc); +} \ No newline at end of file