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 //state variables for ISR
00021 volatile bool run = false;
00022 BusOut start_stop_led(D6, D7);
00023 
00024 //Input to trigger interrupt off of
00025 InterruptIn start_stop_button(SW3);
00026 
00027 //callback fx for ISR
00028 void start_stop()
00029 {
00030     run = !run;
00031     start_stop_led = (start_stop_led ^ 3);
00032 }
00033 
00034 //Function for reading QTR-RC infrared sensor from Pololu
00035 uint8_t get_ir_bus(BusInOut &ir_bus, uint16_t *ir_vals, uint16_t &sample_cnt);
00036 
00037 //constants used with Pololu sensor
00038 const uint16_t MAX_SAMPLE_TIME = 1000;
00039 const uint8_t NUM_SENSORS = 8;
00040 
00041 int main(void)
00042 {
00043     
00044     //Trigger interrupt on falling edge of SW3 and cal start_stop fx
00045     start_stop_button.fall(&start_stop);
00046     
00047     //state indicator, default red for stop
00048     start_stop_led = 2; // D7 on D6 off = red
00049     
00050     //Connect IR sensor to port 4
00051     BusInOut ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
00052     
00053     uint16_t ir_vals[8]; 
00054     uint16_t samples = 0;
00055     
00056     //binary sensor data
00057     uint8_t ir_val = 0;
00058     
00059     //used to measure dt
00060     DigitalOut loop_pulse(D8, 0);
00061     
00062     //MAXREFDES89# object
00063     Max14871_Shield motor_shield(D14, D15, true);// Default config
00064         
00065     // local vars with names that are more meaningful and easier to use than the class name with scope operator
00066     Max14871_Shield::max14871_motor_driver_t r_motor_driver = Max14871_Shield::MD4;
00067     Max14871_Shield::max14871_motor_driver_t l_motor_driver = Max14871_Shield::MD3;
00068     
00069     motor_shield.set_current_regulation_mode(r_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
00070     motor_shield.set_current_regulation_mode(l_motor_driver, Max14871_Shield::RIPPLE_25_EXTERNAL_REF, 2.0);
00071     
00072     int32_t r_duty_cycle = 0;
00073     int32_t l_duty_cycle = 0;
00074     
00075     const int32_t MAX_DUTY_CYCLE = 80;
00076     const int32_t MIN_DUTY_CYCLE = -80;
00077     
00078     const int32_t TARGET_DUTY_CYCLE = 75;  //was 37
00079     
00080     
00081     /***************************************************
00082     |  Control Type |   KP   |     KI     |     KD     |
00083     |---------------|--------|------------|------------|
00084     |        P      | 0.5Kc  |      0     |      0     |
00085     |---------------------------------------------------
00086     |       PI      | 0.45Kc | 1.2KpdT/Pc |      0     |
00087     |---------------|--------|------------|------------|
00088     |       PD      | 0.80Kc |      0     | KpPc/(8dT) |
00089     |---------------|--------|------------|------------|
00090     |      PID      | 0.60Kc |  2KpdT/Pc  | KpPc/(8dT) |
00091     ***************************************************/
00092     
00093     //Kc = critical gain, gain with just P control at which system becomes marginally stable
00094     //Pc = period of oscillation for previous scenario.
00095     
00096     //Set PID terms to 0 if not used/needed
00097     //For values below Kc = 10 and Pc was measured at 0.33 calculated starting points given in comments below
00098     //Ended up decreasing integral term, increasing derivative term and small decrease in proportional term.
00099     int32_t kp = 5; //was 4
00100     int32_t ki = 0; //.0576, divide by 1000 later, was 2
00101     int32_t kd = 0; //156.25, was 250
00102     
00103     //initialize vars
00104     int32_t current_error = 0;
00105     int32_t previous_error = current_error;
00106     int32_t integral = 0;
00107     int32_t derivative = 0;
00108     
00109     //raw sensor data scaled to a useable range for error signal
00110     //            SP - feedback     raw sensor ir_val
00111     const uint8_t ERROR_SIGNAL_P7 = 0xFE; //b11111110
00112     const uint8_t ERROR_SIGNAL_P6 = 0xFC; //b11111100
00113     const uint8_t ERROR_SIGNAL_P5 = 0xFD; //b11111101
00114     const uint8_t ERROR_SIGNAL_P4 = 0xF9; //b11111001
00115     const uint8_t ERROR_SIGNAL_P3 = 0xFB; //b11111011
00116     const uint8_t ERROR_SIGNAL_P2 = 0xF3; //b11110011
00117     const uint8_t ERROR_SIGNAL_P1 = 0xF7; //b11110111
00118     const uint8_t ERROR_SIGNAL_00 = 0xE7; //b11100111, feedback = SP
00119     const uint8_t ERROR_SIGNAL_N1 = 0xEF; //b11101111
00120     const uint8_t ERROR_SIGNAL_N2 = 0xCF; //b11001111
00121     const uint8_t ERROR_SIGNAL_N3 = 0xDF; //b11011111
00122     const uint8_t ERROR_SIGNAL_N4 = 0x9F; //b10011111
00123     const uint8_t ERROR_SIGNAL_N5 = 0xBF; //b10111111
00124     const uint8_t ERROR_SIGNAL_N6 = 0x3F; //b00111111
00125     const uint8_t ERROR_SIGNAL_N7 = 0x7F; //b01111111
00126     
00127     #ifdef TUNE_PID
00128         char response = 'N';
00129         
00130         printf("\nDo you want to change the PID coefficents? 'Y' or 'N': ");
00131         scanf("%c", &response);
00132         
00133         if((response == 'Y') || (response == 'y'))
00134         {
00135             printf("\nCurrent kp = %d, please enter new kp = ", kp);
00136             scanf("%d", &kp);
00137             
00138             printf("\nCurrent ki = %d, please enter new ki = ", ki);
00139             scanf("%d", &ki);
00140             
00141             printf("\nCurrent kd = %d, please enter new kd = ", kd);
00142             scanf("%d", &kd);
00143         }
00144         
00145         printf("\nThe PID coefficents are: ");
00146         printf("\nkp = %d", kp);
00147         printf("\nki = %d", ki);
00148         printf("\nkd = %d\n", kd);
00149     #endif//TUNE_PID
00150     
00151     
00152     //loop time is ~2.86ms
00153     for(;;)
00154     {   
00155         //wait for start_stop button press
00156         while(!run)
00157         {
00158             //get raw ir sensor data
00159             ir_val = ~(get_ir_bus(ir_bus, ir_vals, samples)); 
00160             
00161             printf("\nir_state = 0x%2x \t samples = %d\n\n", ir_val, samples);
00162             for(uint8_t idx = 0; idx < 8; idx++)
00163             {
00164                 printf("%.4d \t", ir_vals[idx]);
00165             }
00166             printf("\n\n");
00167             
00168             wait(1.0);
00169         }
00170         
00171         //mode is set to forward, but duty cycle is still 0
00172         motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
00173         motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
00174         
00175         while(run)
00176         {
00177             //measure dt with scope
00178             loop_pulse = !loop_pulse;
00179             
00180             //get raw ir sensor data
00181             ir_val = ~(get_ir_bus(ir_bus, ir_vals, samples)); 
00182             
00183             //scale feedback
00184             switch(ir_val)
00185             {
00186                 case(ERROR_SIGNAL_P7):
00187                     current_error = 7;
00188                 break;
00189               
00190                 case(ERROR_SIGNAL_P6):
00191                     current_error = 6;
00192                 break;
00193               
00194                 case(ERROR_SIGNAL_P5):
00195                     current_error = 5;
00196                 break;
00197               
00198                 case(ERROR_SIGNAL_P4):
00199                     current_error = 4;
00200                 break;
00201               
00202                 case(ERROR_SIGNAL_P3):
00203                     current_error = 3;
00204                 break;
00205               
00206                 case(ERROR_SIGNAL_P2):
00207                     current_error = 2;
00208                 break;
00209               
00210                 case(ERROR_SIGNAL_P1):
00211                     current_error = 1;
00212                 break;
00213               
00214                 case(ERROR_SIGNAL_00):
00215                     current_error = 0;
00216                 break;
00217               
00218                 case(ERROR_SIGNAL_N1):
00219                     current_error = -1;
00220                 break;
00221               
00222                 case(ERROR_SIGNAL_N2):
00223                     current_error = -2;
00224                 break;
00225               
00226                 case(ERROR_SIGNAL_N3):
00227                     current_error = -3;
00228                 break;
00229               
00230                 case(ERROR_SIGNAL_N4):
00231                     current_error = -4;
00232                 break;
00233               
00234                 case(ERROR_SIGNAL_N5):
00235                     current_error = -5;
00236                 break;
00237               
00238                 case(ERROR_SIGNAL_N6):
00239                     current_error = -6;
00240                 break;
00241               
00242                 case(ERROR_SIGNAL_N7):
00243                     current_error = -7;
00244                 break;
00245 
00246                 default:
00247                     current_error = previous_error;  
00248                 break;    
00249             }
00250             
00251             /*get integral term, if statement helps w/wind-up
00252             
00253               from url in file banner
00254             
00255               If integral wind-up is a problem two common solutions are 
00256               (1) zero the integral, that is set the variable integral 
00257                   equal to zero, every time the error is zero or the 
00258                   error changes sign.  
00259               (2) "Dampen" the integral by multiplying the accumulated 
00260                   integral by a factor less than one when a new integral 
00261                   is calculated. 
00262             */
00263             
00264             if(current_error == 0)
00265             {
00266                 integral = 0;
00267             }
00268             else if(((current_error < 0) && (previous_error > 0)) || 
00269                   ((current_error > 0) && (previous_error < 0)))
00270             {
00271                 integral = 0;
00272             }
00273             else
00274             {
00275                 integral = (integral + current_error);
00276             }
00277             
00278             //get derivative term
00279             derivative = (current_error - previous_error);
00280             
00281             //save current error for next loop
00282             previous_error = current_error;
00283             
00284             //get new duty cycle for right motor
00285             r_duty_cycle = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative));
00286             
00287             //clamp to limits
00288             if((r_duty_cycle > MAX_DUTY_CYCLE) || (r_duty_cycle < MIN_DUTY_CYCLE))
00289             {
00290                 if(r_duty_cycle > MAX_DUTY_CYCLE)
00291                 {
00292                     r_duty_cycle = MAX_DUTY_CYCLE;
00293                 }
00294                 else
00295                 {
00296                     r_duty_cycle = MIN_DUTY_CYCLE;
00297                 }
00298             }
00299        
00300     
00301             //get new duty cycle for left motor  
00302             l_duty_cycle  = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative));
00303             
00304             //clamp to limits
00305             if((l_duty_cycle > MAX_DUTY_CYCLE) || (l_duty_cycle < MIN_DUTY_CYCLE))
00306             {
00307                 if(l_duty_cycle > MAX_DUTY_CYCLE)
00308                 {
00309                     l_duty_cycle = MAX_DUTY_CYCLE;
00310                 }
00311                 else
00312                 {
00313                     l_duty_cycle = MIN_DUTY_CYCLE;
00314                 }
00315             }
00316             
00317             
00318             //update direction and duty cycle for right motor
00319             if(r_duty_cycle < 0)
00320             {
00321                 r_duty_cycle = (r_duty_cycle * -1);
00322                 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::REVERSE);
00323             }
00324             else
00325             {
00326                 motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::FORWARD);
00327             }
00328             motor_shield.set_pwm_duty_cycle(r_motor_driver, ((float) r_duty_cycle)/100.0f);
00329             
00330             //update direction and duty cycle for left motor
00331             if(l_duty_cycle < 0)
00332             {
00333                 l_duty_cycle = (l_duty_cycle * -1);
00334                 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::REVERSE);
00335             }
00336             else
00337             {
00338                 motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
00339             }
00340             motor_shield.set_pwm_duty_cycle(l_motor_driver, ((float) l_duty_cycle)/100.0f);
00341         }
00342       
00343         //shutdown  
00344         motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::COAST);
00345         motor_shield.set_operating_mode(r_motor_driver, Max14871_Shield::COAST);
00346         
00347         //reset all data to initial state
00348         r_duty_cycle = 0;
00349         l_duty_cycle = 0;
00350         
00351         current_error = 0;
00352         previous_error = current_error;
00353     
00354         integral = 0;
00355         derivative = 0;
00356     }
00357 }
00358 
00359 
00360 //Function for reading QTR-RC infrared sensor from Pololu
00361 uint8_t get_ir_bus(BusInOut &ir_bus, uint16_t *ir_vals, uint16_t &sample_cnt)
00362 {
00363     Timer ir_timer; 
00364     uint8_t ir_state = 0;
00365     uint8_t idx = 0;
00366     uint16_t sample_time = 0;
00367     
00368     for(idx = 0; idx < NUM_SENSORS; idx++)
00369     {
00370         //set initial val to max for comparison later
00371         ir_vals[idx] = MAX_SAMPLE_TIME;
00372         
00373         //build write val for discharging caps based on number of sensors
00374         ir_state |= (1 << idx);
00375     }
00376     
00377     //set bus to output
00378     ir_bus.output();
00379     //discharge caps
00380     ir_bus.write(ir_state);
00381     
00382     wait_us(10);
00383     
00384     //set bus to input
00385     ir_bus.input();
00386     //ensure no pull-up-down resistors to interfere with reading
00387     ir_bus.mode(PullNone);
00388     
00389     //clear sample count and timer
00390     sample_cnt = 0;
00391     ir_timer.reset();
00392     ir_timer.start();
00393     
00394     do
00395     {
00396         sample_cnt++;
00397         //get sample time from start in micro seconds
00398         sample_time = ir_timer.read_us();
00399         //get bus state
00400         ir_state = ir_bus.read();
00401         
00402         for(idx = 0; idx < NUM_SENSORS; idx++)
00403         {
00404             //if pin state switched to 'low', record sample time
00405             if(!(ir_state & (1 << idx)) && (ir_vals[idx] > sample_time))
00406             {
00407                 ir_vals[idx] = sample_time;
00408             }
00409         }
00410     }
00411     while(sample_time < MAX_SAMPLE_TIME);
00412     
00413     ir_timer.stop();
00414     
00415     return(ir_state);
00416 }