Line following bot using MAXREFDES89 and MAX32600MBED
Dependencies: MAX14871_Shield mbed
main.cpp
- Committer:
- j3
- Date:
- 2015-12-14
- Revision:
- 0:d2693f27f344
- Child:
- 1:b928ca54cd1a
File content as of revision 0:d2693f27f344:
/********************************************************************** * 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); }