Diff: main.cpp

Revision:

0:929eee0b13f2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 14 17:14:36 2018 +0000
@@ -0,0 +1,142 @@
+#include "mbed.h"
+#include "PID.h"
+#include "QEI.h"
+
+/*
+    Demo of setting motor speed using encoder in pid feedback. 
+    
+    Parts used for this demo:
+    HN-GH12-1634T Gear Motor
+    LMD18200 H-Bridge Breakout Board
+    E4P-100-079 Quadrature Encoder
+    
+    TODO: Implement a "rolling"/"moving" average in callback 
+    for speed feedback
+*/
+
+float clip(float value, float lower, float upper);
+
+static float setpoint, feedback, output;
+const float output_lower_limit = -1.0;          
+const float output_upper_limit = 1.0;
+const float FEEDBACK_SCALE = 1.0/3000.0;        // Scale feedback to 1rev/3000cnts
+
+const float kp = 0.01;
+const float ki = 0.01;
+const float kd = 0.0;
+const float Ts = 0.04;                          // 25Hz Sample Freq (40ms Sample Time);
+                                                // Used for PID Sample time and used
+                                                // to calculate callback() interrupt
+                                                // time                                                
+
+PID pid(&setpoint, &feedback, &output, output_lower_limit, output_upper_limit,
+        kp, ki,  kd, Ts);
+QEI encoder(p15, p16);
+PwmOut mtr_pwm(p25);
+DigitalOut mtr_dir(p24);
+void pid_callback();                               // Updates encoder feedback and motor output
+Ticker motor;
+
+int main() {
+    // Wait for me to plug in motor battery
+    wait(5);
+    
+    // Clear encoder count
+    encoder.reset();
+    
+    // Init the motor
+    mtr_dir = 0;            // CW
+    mtr_pwm = 0.0;   // Quarter speed
+    
+    // Update sensors and feedback twice as fast as PID sample time;
+    // this makes pid react in real-time avoiding errors due to 
+    // missing counts etc. 
+    motor.attach(&pid_callback, Ts/2.0);
+    
+    // Init the pid
+    /*TODO: Implement PID Change Param Method in the PID class
+    and use it to init gains here*/
+    setpoint = 0.0;
+    feedback = encoder.read();
+    output = 0.0;
+    pid.start();
+
+    while(1){
+        int flag;
+        float userInput;
+        do{ 
+            printf("Enter Speed/RPM (-100.0 to 100.0)\r\n");
+            flag = scanf("%f", &userInput);
+        }while(flag == EOF);
+        setpoint = userInput;
+        do{   
+            printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n", 
+                    setpoint, feedback, pid.getError(), output);
+            wait(0.25);
+        }while(pid.getError() < -0.006 || 0.006 < pid.getError());
+        printf("Speed Reached!\r\n");
+        printf("Setpoint: %1.2f\t\tFeedback: %1.2f\t\tError: %1.2f\t\tOuput: %1.2f\r\n", 
+                setpoint, feedback, pid.getError(), output);
+    }
+}
+
+/*
+    Updates feedback and output, interrupt driven so that paramaters
+    are updated in real-time, i.e. avoids update lag due to main
+    code overhead and printfs which can be slow.
+*/
+void pid_callback(){
+    // Update motor
+    if(setpoint >= 0.0) mtr_dir = 1;       // Set motor direction based on setpoint
+    else mtr_dir = 0;
+    if(-0.001 < setpoint && setpoint < 0.001){ 
+        /* Setpoint = 0 is a special case, we allow output to control speed AND 
+        direction to fight intertia and/or downhill roll. */
+        if(output >= 0.0) mtr_dir = 1;
+        else mtr_dir = 0;
+        mtr_pwm = abs(output);
+    }
+    else{    
+        if(mtr_dir == 1){                      // If CW then apply positive outputs
+            if(output >= 0.0) mtr_pwm = output;
+            else mtr_pwm = 0.0;
+        }
+        else{                                // If CCW then apply negative outputs
+            if(output <= 0.0) mtr_pwm = abs(output);
+            else mtr_pwm = 0.0;
+        }                          
+    } 
+//    if(mtr_dir == 1){                     // If CW then apply positive outputs
+//        if(output >= 0.0) mtr_pwm = output;
+//        else mtr_pwm = mtr_pwm.read() - abs(output); // Take negative output value out of full range
+//                                          // helps avoid bumpiness in motor
+//    }
+//    else{                                // If CCW then apply negative outputs
+//        if(output <= 0.0) mtr_pwm = abs(output);
+//        else mtr_pwm = mtr_pwm.read() - abs(output);
+//    }                          
+//        
+    // Running average
+    float k = Ts/2.0;                        // Discrete time, (Ts/2 because this callback is called
+                                             // at interval of Ts/2... or twice as fast as pid controller)
+    
+    /* TODO: Implement a "rolling"/"moving" average */
+    static int last_count = 0;
+    int count = encoder.read();
+    float raw_speed = ((count - last_count)*FEEDBACK_SCALE) / k; 
+    float rpm_speed = raw_speed * 60.0;     // Convert speed to RPM
+    
+    last_count = count;                     // Save last count
+    feedback = rpm_speed;              
+}
+
+/*
+    Clips value to lower/ uppper
+    @param value    The value to clip
+    @param lower    The mininum allowable value
+    @param upper    The maximum allowable value
+    @return         The resulting clipped value
+*/
+float clip(float value, float lower, float upper){
+    return std::max(lower, std::min(value, upper));
+}
\ No newline at end of file