Line following bot using MAXREFDES89 and MAX32600MBED
Dependencies: MAX14871_Shield mbed
Diff: main.cpp
- Revision:
- 4:eb6d6d25355c
- Parent:
- 3:d268f6e06b7a
- Child:
- 5:c673430c8a32
--- a/main.cpp Mon Dec 21 00:40:11 2015 +0000 +++ b/main.cpp Wed Jan 06 22:57:35 2016 +0000 @@ -9,16 +9,17 @@ * **********************************************************************/ + #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); +//comment out following line for normal operation +//#define TUNE_PID 1 //state variables for ISR -bool run = false; +volatile bool run = false; BusOut start_stop_led(D6, D7); //Input to trigger interrupt off of @@ -34,26 +35,41 @@ 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 + //raw 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 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 = 100; const int32_t MIN_DUTY_CYCLE = -100; + const int32_t TARGET_DUTY_CYCLE = 35; + /*************************************************** | Control Type | KP | KI | KD | @@ -69,19 +85,23 @@ //Kc = critical gain, gain with just P control at which system becomes marginally stable //Pc = period of oscillation for previous scenario. + //for values below Kc = 10 and Pc was measured at 0.33 //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; + //calculated starting points given in comments below + //Ended up decreasing integral term, increasing derivative term and small decrease in proportional term. + int32_t kp = 5; //6 + int32_t ki = 10; //.0576, divide by 1000 later + int32_t kd = 500; //156.25 - //raw sensor data - uint8_t ir_val = 0; + //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 + //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 @@ -109,15 +129,35 @@ //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; + #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); @@ -127,8 +167,10 @@ while(run) { + //measure dt with scope loop_pulse = !loop_pulse; + //get raw ir sensor data ir_val = ~(ir_bus.read()); //scale feedback @@ -193,7 +235,7 @@ case(ERROR_SIGNAL_N7): current_error = -7; break; - + case(ERROR_LT0): current_error = 7; break; @@ -219,17 +261,28 @@ break; case(ERROR_OFF_TRACK): - //do circles cause I am lost - current_error = 7; - break; - + //requires reset + motor_shield.set_pwm_duty_cycle(r_motor_driver, 0.0f); + motor_shield.set_pwm_duty_cycle(l_motor_driver, 0.0f); + while(1); default: current_error = previous_error; break; } + /*get integral term, if statement helps w/wind-up - //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; @@ -251,15 +304,37 @@ previous_error = current_error; //get new duty cycle for right motor - r_duty_cycle = (TARGET_DUTY_CYCLE - (KP*current_error + (KI*integral) + KD*derivative)); + r_duty_cycle = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative)); + //clamp to limits - r_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, r_duty_cycle ); + 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) + KD*derivative)); + l_duty_cycle = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative)); + //clamp to limits - l_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, l_duty_cycle ); + 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 @@ -306,19 +381,3 @@ 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); -} \ No newline at end of file