Line following bot using MAXREFDES89 and MAX32600MBED
Dependencies: MAX14871_Shield mbed
main.cpp
- Committer:
- j3
- Date:
- 2016-02-16
- Revision:
- 6:428538df7b63
- Parent:
- 5:c673430c8a32
File content as of revision 6:428538df7b63:
/********************************************************************** * 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" //comment out following line for normal operation //#define TUNE_PID 1 //state variables for ISR volatile 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) { //Trigger interrupt on falling edge of SW3 and cal start_stop fx start_stop_button.fall(&start_stop); //state indicator, default red for stop start_stop_led = 2; // D7 on D6 off = red //Connect IR sensor to port 4 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 //binary sensor data uint8_t ir_val = 0; //used to measure dt DigitalOut loop_pulse(D8, 0); //MAXREFDES89# object Max14871_Shield motor_shield(D14, D15, true);// Default config // local vars with names that are more meaningful and easier to use than the class name with scope operator Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4; Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3; motor_shield.set_current_regulation_mode(r_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0); motor_shield.set_current_regulation_mode(l_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0); int32_t r_duty_cycle = 0; int32_t l_duty_cycle = 0; const int32_t MAX_DUTY_CYCLE = 80; const int32_t MIN_DUTY_CYCLE = -80; const int32_t TARGET_DUTY_CYCLE = 37; /*************************************************** | 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 //For values below Kc = 10 and Pc was measured at 0.33 calculated starting points given in comments below //Ended up decreasing integral term, increasing derivative term and small decrease in proportional term. int32_t kp = 4; // int32_t ki = 2; //.0576, divide by 1000 later int32_t kd = 250; //156.25 //initialize vars int32_t current_error = 0; int32_t previous_error = current_error; int32_t integral = 0; int32_t derivative = 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 #ifdef TUNE_PID char response = 'N'; printf("\nDo you want to change the PID coefficents? 'Y' or 'N': "); scanf("%c", &response); if((response == 'Y') || (response == 'y')) { printf("\nCurrent kp = %d, please enter new kp = ", kp); scanf("%d", &kp); printf("\nCurrent ki = %d, please enter new ki = ", ki); scanf("%d", &ki); printf("\nCurrent kd = %d, please enter new kd = ", kd); scanf("%d", &kd); } printf("\nThe PID coefficents are: "); printf("\nkp = %d", kp); printf("\nki = %d", ki); printf("\nkd = %d\n", kd); #endif//TUNE_PID //loop time is ~1.6ms 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) { //measure dt with scope loop_pulse = !loop_pulse; //get raw ir sensor data ir_val = ~(ir_bus.read()); // use with Parallax Sensor //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; default: current_error = previous_error; break; } /*get integral term, if statement helps w/wind-up from url in file banner If integral wind-up is a problem two common solutions are (1) zero the integral, that is set the variable integral equal to zero, every time the error is zero or the error changes sign. (2) "Dampen" the integral by multiplying the accumulated integral by a factor less than one when a new integral is calculated. */ 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)/1000) + kd*derivative)); //clamp to limits if((r_duty_cycle > MAX_DUTY_CYCLE) || (r_duty_cycle < MIN_DUTY_CYCLE)) { if(r_duty_cycle > MAX_DUTY_CYCLE) { r_duty_cycle = MAX_DUTY_CYCLE; } else { r_duty_cycle = MIN_DUTY_CYCLE; } } //get new duty cycle for left motor l_duty_cycle = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative)); //clamp to limits if((l_duty_cycle > MAX_DUTY_CYCLE) || (l_duty_cycle < MIN_DUTY_CYCLE)) { if(l_duty_cycle > MAX_DUTY_CYCLE) { l_duty_cycle = MAX_DUTY_CYCLE; } else { l_duty_cycle = MIN_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); } 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); } 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; } }