main.cpp

00001 /**********************************************************************
00002 * Simple line following bot with PID to demo MAXREFDES89#
00003 * PID feedback comes from infrared led sensor from Parallax
00004 * https://www.parallax.com/product/28034
00005 *
00006 * The following link is a good example for a PID line follower
00007 *
00008 * http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html
00009 *
00010 **********************************************************************/
00011 
00012 
00013 #include "mbed.h"
00014 #include "max14871_shield.h"
00015 
00016 
00017 //comment out following line for normal operation
00018 //#define TUNE_PID 1
00019 
00020 
00021 //state variables for ISR
00022 volatile bool run = false;
00023 BusOut start_stop_led(D6, D7);
00024 
00025 //Input to trigger interrupt off of
00026 InterruptIn start_stop_button(SW3);
00027 
00028 //callback fx for ISR
00029 void start_stop()
00030 {
00031     run = !run;
00032     start_stop_led = (start_stop_led ^ 3);
00033 }
00034 
00035 
00036 int main(void)
00037 {
00038     
00039     //Trigger interrupt on falling edge of SW3 and cal start_stop fx
00040     start_stop_button.fall(&start_stop);
00041     
00042     //state indicator, default red for stop
00043     start_stop_led = 2; // D7 on D6 off = red
00044     
00045     //Connect IR sensor to port 4
00046     BusIn ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
00047     
00048     DigitalOut ir_bus_enable(D3, 1); //active high enable
00049     
00050     //binary sensor data
00051     uint8_t ir_val = 0;
00052     
00053     //used to measure dt
00054     DigitalOut loop_pulse(D8, 0);
00055     
00056     //MAXREFDES89# object
00057     Max14871_Shield motor_shield(D14, D15, true);// Default config
00058         
00059     // local vars with names that are more meaningful and easier to use than the class name with scope operator
00060     Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4;
00061     Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3;
00062     
00063     motor_shield.set_current_regulation_mode(r_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
00064     motor_shield.set_current_regulation_mode(l_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
00065     
00066     int32_t r_duty_cycle = 0;
00067     int32_t l_duty_cycle = 0;
00068     
00069     const int32_t MAX_DUTY_CYCLE = 80;
00070     const int32_t MIN_DUTY_CYCLE = -80;
00071     
00072     const int32_t TARGET_DUTY_CYCLE = 37;
00073     
00074     
00075     /***************************************************
00076     |  Control Type |   KP   |     KI     |     KD     |
00077     |---------------|--------|------------|------------|
00078     |        P      | 0.5Kc  |      0     |      0     |
00079     |---------------------------------------------------
00080     |       PI      | 0.45Kc | 1.2KpdT/Pc |      0     |
00081     |---------------|--------|------------|------------|
00082     |       PD      | 0.80Kc |      0     | KpPc/(8dT) |
00083     |---------------|--------|------------|------------|
00084     |      PID      | 0.60Kc |  2KpdT/Pc  | KpPc/(8dT) |
00085     ***************************************************/
00086     
00087     //Kc = critical gain, gain with just P control at which system becomes marginally stable
00088     //Pc = period of oscillation for previous scenario.
00089     
00090     //Set PID terms to 0 if not used/needed
00091     //For values below Kc = 10 and Pc was measured at 0.33 calculated starting points given in comments below
00092     //Ended up decreasing integral term, increasing derivative term and small decrease in proportional term.
00093     int32_t kp = 4; //
00094     int32_t ki = 2; //.0576, divide by 1000 later
00095     int32_t kd = 250; //156.25
00096     
00097     //initialize vars
00098     int32_t current_error = 0;
00099     int32_t previous_error = current_error;
00100     int32_t integral = 0;
00101     int32_t derivative = 0;
00102     
00103     //raw sensor data scaled to a useable range for error signal
00104     //            SP - feedback     raw sensor ir_val
00105     const uint8_t ERROR_SIGNAL_P7 = 0xFE; //b11111110
00106     const uint8_t ERROR_SIGNAL_P6 = 0xFC; //b11111100
00107     const uint8_t ERROR_SIGNAL_P5 = 0xFD; //b11111101
00108     const uint8_t ERROR_SIGNAL_P4 = 0xF9; //b11111001
00109     const uint8_t ERROR_SIGNAL_P3 = 0xFB; //b11111011
00110     const uint8_t ERROR_SIGNAL_P2 = 0xF3; //b11110011
00111     const uint8_t ERROR_SIGNAL_P1 = 0xF7; //b11110111
00112     const uint8_t ERROR_SIGNAL_00 = 0xE7; //b11100111, feedback = SP
00113     const uint8_t ERROR_SIGNAL_N1 = 0xEF; //b11101111
00114     const uint8_t ERROR_SIGNAL_N2 = 0xCF; //b11001111
00115     const uint8_t ERROR_SIGNAL_N3 = 0xDF; //b11011111
00116     const uint8_t ERROR_SIGNAL_N4 = 0x9F; //b10011111
00117     const uint8_t ERROR_SIGNAL_N5 = 0xBF; //b10111111
00118     const uint8_t ERROR_SIGNAL_N6 = 0x3F; //b00111111
00119     const uint8_t ERROR_SIGNAL_N7 = 0x7F; //b01111111
00120     
00121     #ifdef TUNE_PID
00122         char response = 'N';
00123         
00124         printf("\nDo you want to change the PID coefficents? 'Y' or 'N': ");
00125         scanf("%c", &response);
00126         
00127         if((response == 'Y') || (response == 'y'))
00128         {
00129             printf("\nCurrent kp = %d, please enter new kp = ", kp);
00130             scanf("%d", &kp);
00131             
00132             printf("\nCurrent ki = %d, please enter new ki = ", ki);
00133             scanf("%d", &ki);
00134             
00135             printf("\nCurrent kd = %d, please enter new kd = ", kd);
00136             scanf("%d", &kd);
00137         }
00138         
00139         printf("\nThe PID coefficents are: ");
00140         printf("\nkp = %d", kp);
00141         printf("\nki = %d", ki);
00142         printf("\nkd = %d\n", kd);
00143     #endif//TUNE_PID
00144     
00145     
00146     //loop time is ~1.6ms
00147     for(;;)
00148     {   
00149         //wait for start_stop button press
00150         while(!run);
00151         
00152         //mode is set to forward, but duty cycle is still 0
00153         motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
00154         motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
00155         
00156         while(run)
00157         {
00158             //measure dt with scope
00159             loop_pulse = !loop_pulse;
00160             
00161             //get raw ir sensor data
00162             ir_val = ~(ir_bus.read()); // use with Parallax Sensor
00163             
00164             //scale feedback
00165             switch(ir_val)
00166             {
00167                 case(ERROR_SIGNAL_P7):
00168                     current_error = 7;
00169                 break;
00170               
00171                 case(ERROR_SIGNAL_P6):
00172                     current_error = 6;
00173                 break;
00174               
00175                 case(ERROR_SIGNAL_P5):
00176                     current_error = 5;
00177                 break;
00178               
00179                 case(ERROR_SIGNAL_P4):
00180                     current_error = 4;
00181                 break;
00182               
00183                 case(ERROR_SIGNAL_P3):
00184                     current_error = 3;
00185                 break;
00186               
00187                 case(ERROR_SIGNAL_P2):
00188                     current_error = 2;
00189                 break;
00190               
00191                 case(ERROR_SIGNAL_P1):
00192                     current_error = 1;
00193                 break;
00194               
00195                 case(ERROR_SIGNAL_00):
00196                     current_error = 0;
00197                 break;
00198               
00199                 case(ERROR_SIGNAL_N1):
00200                     current_error = -1;
00201                 break;
00202               
00203                 case(ERROR_SIGNAL_N2):
00204                     current_error = -2;
00205                 break;
00206               
00207                 case(ERROR_SIGNAL_N3):
00208                     current_error = -3;
00209                 break;
00210               
00211                 case(ERROR_SIGNAL_N4):
00212                     current_error = -4;
00213                 break;
00214               
00215                 case(ERROR_SIGNAL_N5):
00216                     current_error = -5;
00217                 break;
00218               
00219                 case(ERROR_SIGNAL_N6):
00220                     current_error = -6;
00221                 break;
00222               
00223                 case(ERROR_SIGNAL_N7):
00224                     current_error = -7;
00225                 break;
00226 
00227                 default:
00228                     current_error = previous_error;  
00229                 break;    
00230             }
00231             
00232             /*get integral term, if statement helps w/wind-up
00233             
00234               from url in file banner
00235             
00236               If integral wind-up is a problem two common solutions are 
00237               (1) zero the integral, that is set the variable integral 
00238                   equal to zero, every time the error is zero or the 
00239                   error changes sign.  
00240               (2) "Dampen" the integral by multiplying the accumulated 
00241                   integral by a factor less than one when a new integral 
00242                   is calculated. 
00243             */
00244             
00245             if(current_error == 0)
00246             {
00247                 integral = 0;
00248             }
00249             else if(((current_error < 0) && (previous_error > 0)) || 
00250                   ((current_error > 0) && (previous_error < 0)))
00251             {
00252                 integral = 0;
00253             }
00254             else
00255             {
00256                 integral = (integral + current_error);
00257             }
00258             
00259             //get derivative term
00260             derivative = (current_error - previous_error);
00261             
00262             //save current error for next loop
00263             previous_error = current_error;
00264             
00265             //get new duty cycle for right motor
00266             r_duty_cycle = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative));
00267             
00268             //clamp to limits
00269             if((r_duty_cycle > MAX_DUTY_CYCLE) || (r_duty_cycle < MIN_DUTY_CYCLE))
00270             {
00271                 if(r_duty_cycle > MAX_DUTY_CYCLE)
00272                 {
00273                     r_duty_cycle = MAX_DUTY_CYCLE;
00274                 }
00275                 else
00276                 {
00277                     r_duty_cycle = MIN_DUTY_CYCLE;
00278                 }
00279             }
00280        
00281     
00282             //get new duty cycle for left motor  
00283             l_duty_cycle  = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative));
00284             
00285             //clamp to limits
00286             if((l_duty_cycle > MAX_DUTY_CYCLE) || (l_duty_cycle < MIN_DUTY_CYCLE))
00287             {
00288                 if(l_duty_cycle > MAX_DUTY_CYCLE)
00289                 {
00290                     l_duty_cycle = MAX_DUTY_CYCLE;
00291                 }
00292                 else
00293                 {
00294                     l_duty_cycle = MIN_DUTY_CYCLE;
00295                 }
00296             }
00297             
00298             
00299             //update direction and duty cycle for right motor
00300             if(r_duty_cycle < 0)
00301             {
00302                 r_duty_cycle = (r_duty_cycle * -1);
00303                 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::REVERSE);
00304             }
00305             else
00306             {
00307                 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
00308             }
00309             motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
00310             
00311             //update direction and duty cycle for left motor
00312             if(l_duty_cycle < 0)
00313             {
00314                 l_duty_cycle = (l_duty_cycle * -1);
00315                 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::REVERSE);
00316             }
00317             else
00318             {
00319                 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
00320             }
00321             motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
00322         }
00323       
00324         //shutdown  
00325         motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::COAST);
00326         motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::COAST);
00327         
00328         //reset all data to initial state
00329         r_duty_cycle = 0;
00330         l_duty_cycle = 0;
00331         
00332         current_error = 0;
00333         previous_error = current_error;
00334     
00335         integral = 0;
00336         derivative = 0;
00337     }
00338 }