main.cpp

00001 #include "mbed.h"
00002 #include "PID.h"
00003 #include "QEI.h"
00004 
00005 /*
00006     Demo of setting motor speed using encoder in pid feedback. 
00007     
00008     Parts used for this demo:
00009     HN-GH12-1634T Gear Motor
00010     LMD18200 H-Bridge Breakout Board
00011     E4P-100-079 Quadrature Encoder
00012     
00013     TODO: Implement a "rolling"/"moving" average in callback 
00014     for speed feedback
00015 */
00016 
00017 float clip(float value, float lower, float upper);
00018 
00019 static float setpoint, feedback, output;
00020 const float output_lower_limit = -1.0;          
00021 const float output_upper_limit = 1.0;
00022 const float FEEDBACK_SCALE = 1.0/3000.0;        // Scale feedback to 1rev/3000cnts
00023 
00024 const float kp = 0.01;
00025 const float ki = 0.01;
00026 const float kd = 0.0;
00027 const float Ts = 0.04;                          // 25Hz Sample Freq (40ms Sample Time);
00028                                                 // Used for PID Sample time and used
00029                                                 // to calculate callback() interrupt
00030                                                 // time                                                
00031 
00032 PID pid(&setpoint, &feedback, &output, output_lower_limit, output_upper_limit,
00033         kp, ki,  kd, Ts);
00034 QEI encoder(p15, p16);
00035 PwmOut mtr_pwm(p25);
00036 DigitalOut mtr_dir(p24);
00037 void pid_callback();                               // Updates encoder feedback and motor output
00038 Ticker motor;
00039 
00040 int main() {
00041     // Wait for me to plug in motor battery
00042     wait(5);
00043     
00044     // Clear encoder count
00045     encoder.reset();
00046     
00047     // Init the motor
00048     mtr_dir = 0;            // CW
00049     mtr_pwm = 0.0;   // Quarter speed
00050     
00051     // Update sensors and feedback twice as fast as PID sample time;
00052     // this makes pid react in real-time avoiding errors due to 
00053     // missing counts etc. 
00054     motor.attach(&pid_callback, Ts/2.0);
00055     
00056     // Init the pid
00057     /*TODO: Implement PID Change Param Method in the PID class
00058     and use it to init gains here*/
00059     setpoint = 0.0;
00060     feedback = encoder.read();
00061     output = 0.0;
00062     pid.start();
00063 
00064     while(1){
00065         int flag;
00066         float userInput;
00067         do{ 
00068             printf("Enter Speed/RPM (-100.0 to 100.0)\r\n");
00069             flag = scanf("%f", &userInput);
00070         }while(flag == EOF);
00071         setpoint = userInput;
00072         do{   
00073             printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n", 
00074                     setpoint, feedback, pid.getError(), output);
00075             wait(0.25);
00076         }while(pid.getError() < -0.006 || 0.006 < pid.getError());
00077         printf("Speed Reached!\r\n");
00078         printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n", 
00079                 setpoint, feedback, pid.getError(), output);
00080     }
00081 }
00082 
00083 /*
00084     Updates feedback and output, interrupt driven so that paramaters
00085     are updated in real-time, i.e. avoids update lag due to main
00086     code overhead and printfs which can be slow.
00087 */
00088 void pid_callback(){
00089     // Update motor
00090     if(setpoint >= 0.0) mtr_dir = 1;       // Set motor direction based on setpoint
00091     else mtr_dir = 0;
00092     if(-0.001 < setpoint && setpoint < 0.001){ 
00093         /* Setpoint = 0 is a special case, we allow output to control speed AND 
00094         direction to fight intertia and/or downhill roll. */
00095         if(output >= 0.0) mtr_dir = 1;
00096         else mtr_dir = 0;
00097         mtr_pwm = abs(output);
00098     }
00099     else{    
00100         if(mtr_dir == 1){                      // If CW then apply positive outputs
00101             if(output >= 0.0) mtr_pwm = output;
00102             else mtr_pwm = 0.0;
00103         }
00104         else{                                // If CCW then apply negative outputs
00105             if(output <= 0.0) mtr_pwm = abs(output);
00106             else mtr_pwm = 0.0;
00107         }                          
00108     } 
00109 //    if(mtr_dir == 1){                     // If CW then apply positive outputs
00110 //        if(output >= 0.0) mtr_pwm = output;
00111 //        else mtr_pwm = mtr_pwm.read() - abs(output); // Take negative output value out of full range
00112 //                                          // helps avoid bumpiness in motor
00113 //    }
00114 //    else{                                // If CCW then apply negative outputs
00115 //        if(output <= 0.0) mtr_pwm = abs(output);
00116 //        else mtr_pwm = mtr_pwm.read() - abs(output);
00117 //    }                          
00118 //        
00119     // Running average
00120     float k = Ts/2.0;                        // Discrete time, (Ts/2 because this callback is called
00121                                              // at interval of Ts/2... or twice as fast as pid controller)
00122     
00123     /* TODO: Implement a "rolling"/"moving" average */
00124     static int last_count = 0;
00125     int count = encoder.read();
00126     float raw_speed = ((count - last_count)*FEEDBACK_SCALE) / k; 
00127     float rpm_speed = raw_speed * 60.0;     // Convert speed to RPM
00128     
00129     last_count = count;                     // Save last count
00130     feedback = rpm_speed;              
00131 }
00132 
00133 /*
00134     Clips value to lower/ uppper
00135     @param value    The value to clip
00136     @param lower    The mininum allowable value
00137     @param upper    The maximum allowable value
00138     @return         The resulting clipped value
00139 */
00140 float clip(float value, float lower, float upper){
00141     return std::max(lower, std::min(value, upper));
00142 }