Line following bot using MAXREFDES89, MAX32600MBED, and Pololu QTR-RC sensor
Dependencies: MAX14871_Shield mbed
Diff: main.cpp
- Revision:
- 0:d2693f27f344
- Child:
- 1:b928ca54cd1a
diff -r 000000000000 -r d2693f27f344 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Dec 14 00:03:54 2015 +0000 @@ -0,0 +1,299 @@ +/********************************************************************** +* Simple line following bot with PID to demo MAXREFDES89# +* PID feedback comes from infrared led sensor from Parallax +* https://www.parallax.com/product/28034 +**********************************************************************/ + +#include "mbed.h" +#include "max14871_shield.h" + + +float ramp(float val, float val_old); + +bool run = false; +DigitalOut run_led(LED_GREEN, 1); //leds have inverted logic +DigitalOut stop_led(LED_RED, 0); + +InterruptIn start_stop_button(SW3); + +void start_stop() +{ + run = !run; + run_led = !run_led; + stop_led = !stop_led; +} + + +int main(void) +{ + start_stop_button.fall(&start_stop); + + 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 + + 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; + + //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; + + //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; + + 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; + + for(;;) + { + //wait for start_stop button press + while(!run); + + //mode is set to forward, but duty cycle is still 0.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) + { + ir_val = ~(ir_bus.read()); + + //scale feedback + switch(ir_val) + { + case(ZONE_8): + if(fb < 0) + { + fb = -8; + } + else + { + fb = 8; + } + break; + + case(ZONE_7): + fb = 7; + break; + + case(ZONE_6): + fb = 6; + break; + + case(ZONE_5): + fb = 5; + break; + + case(ZONE_4): + fb = 4; + break; + + case(ZONE_3): + fb = 3; + break; + + case(ZONE_2): + fb = 2; + break; + + case(ZONE_1): + fb = 1; + break; + + case(ZONE_0): + fb = 0; + break; + + case(ZONE_N1): + fb = -1; + break; + + case(ZONE_N2): + fb = -2; + break; + + case(ZONE_N3): + fb = -3; + break; + + case(ZONE_N4): + fb = -4; + break; + + case(ZONE_N5): + fb = -5; + break; + + case(ZONE_N6): + fb = -6; + break; + + case(ZONE_N7): + fb = -7; + break; + + case(ZONE_LT0): + fb = 8; + break; + + case(ZONE_LT1): + fb = 8; + break; + + case(ZONE_LT2): + fb = 8; + break; + + case(ZONE_RT0): + fb = -8; + break; + + case(ZONE_RT1): + fb = -8; + break; + + case(ZONE_RT2): + fb = -8; + break; + + default: + fb = fb; + } + + //get error signal + current_error = SP-fb; + + //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 = ((KP*current_error) + (KI*integral) + (KD*derivative)); + if(r_duty_cycle > MAX_DUTY_CYCLE) + { + r_duty_cycle = MAX_DUTY_CYCLE; + } + + if(r_duty_cycle < MIN_DUTY_CYCLE) + { + r_duty_cycle = MIN_DUTY_CYCLE; + } + + //get new duty cycle for left motor + l_duty_cycle = ((KP*current_error) + (KI*integral) + (KD*derivative)); + if(l_duty_cycle > MAX_DUTY_CYCLE) + { + l_duty_cycle = MAX_DUTY_CYCLE; + } + + if(l_duty_cycle < MIN_DUTY_CYCLE) + { + l_duty_cycle = MIN_DUTY_CYCLE; + } + + //ramp motors + do + { + 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)); + } + + //shutdown + motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::BRAKE); + motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::BRAKE); + } +} + + +float ramp(float val, float val_old) +{ + if(val > val_old) + { + if((val - val_old) >= 0.1) + { + val_old += 0.1; + } + else + { + val_old = val; + } + } + else if(val < val_old) + { + if((val_old - val) >= 0.1) + { + val_old -= 0.1; + } + else + { + val_old = val; + } + } + else + { + val_old = val; + } + + return(val_old); +}