Revision 0:31ccfbaf5bff, committed 2018-08-02

Comitter:

snaranjo

Date:

Thu Aug 02 14:46:49 2018 +0000

Commit message:

Simple PID control with a DC motor, attached QEI, and potentiometer.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Basic_SM_PID.cpp	Thu Aug 02 14:46:49 2018 +0000
@@ -0,0 +1,170 @@
+#include "mbed.h"
+#include "QEI.h"
+
+#define pi  3.14159265358979323846
+
+
+// typedef struct
+ struct sPID
+{
+    double dState; // Last position input
+    double iState; // Integrator state
+    double iMax, iMin; // Maximum and minimum allowable integrator state
+    double iGain, // integral gain
+           pGain, // proportional gain
+           dGain; // derivative gain
+};
+
+
+
+double UpdatePID(sPID * pid, double error, double position)
+{
+    double pTerm, dTerm, iTerm;
+    
+    pTerm = pid->pGain * error; // calculate the proportional term
+    
+   
+    pid->iState += error;  // calculate the integral state with appropriate limiting
+
+    if (pid->iState > pid->iMax) pid->iState = pid->iMax;
+    else if (pid->iState < pid->iMin) pid->iState = pid->iMin;
+
+    iTerm = pid->iGain * pid->iState; // calculate the integral term
+    dTerm = pid->dGain * (pid->dState - position);
+
+    pid->dState = position;
+
+    return pTerm + dTerm + iTerm;
+}
+
+void setPID(sPID *pid, double P, double I, double D,
+                      double iMx, double iMn, double dS, double iS){
+ 
+    pid->pGain = P;
+    pid->iGain = I;
+    pid->dGain = D;
+    pid->iMax = iMx;
+    pid->iMin = iMn;
+    pid->dState = dS;
+    pid->iState = iS;
+ 
+ }
+ 
+Serial pc(USBTX, USBRX);
+
+int baudRate = 460800;
+
+
+
+int steps_per_rev = 1200; //Encoder CPR * gearbox ratio
+int pulseRead = 0;
+
+float ctrlDeg = 0.0;
+float shaftDeg = 0.0;
+float cmdSig = 0.0;
+float degErr = 0.0;
+
+Timer timer;
+
+//Use X4 encoding.
+QEI wheel(p11, p12, NC, steps_per_rev, QEI::X4_ENCODING);       //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))
+
+//Output to motors. PWM signal is sent to either one, depending on desired rotational direction. CW or CCW.
+DigitalOut cwCH(p22);             //Ouput pin to h-bridge 1
+DigitalOut ccwCH(p26);
+PwmOut motor(p24);            //Output pin to h-bridge 2
+AnalogIn Current (p19);         //Current sense pin
+AnalogIn ctrlPot (p16);
+AnalogIn currPot (p18);
+
+
+float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
+{
+    return ((pulses/steps_per_rev)*(2*pi));
+}
+
+float pulsesToDegrees(float pulses) //Encoder steps to revolutions of output shaft in degrees
+{
+
+    return ((pulses/steps_per_rev)*360);
+}
+
+void signal_to_hbridge( float signal)   //Input of -1 means full reverse, 1 means full forward
+{
+    //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write() function
+    if (signal < 0) {
+          cwCH  = 0;
+          ccwCH = 1;
+          motor.write(signal * -1);
+//        Motor1.write(0);
+//        Motor2.write(signal * -1);
+    } else if (signal > 0) {
+          ccwCH = 0;
+          cwCH  = 1; 
+          motor.write(signal);
+//        Motor1.write(signal);
+//        Motor2.write(0);
+    } else {
+        cwCH = 0;
+        ccwCH = 0;
+//        Motor1.write(0);
+//        Motor2.write(0);
+    }
+}
+
+
+int main() {
+      
+      pc.baud(baudRate); 
+      //will run a loop to apply the PID controller. I will command a movement with the 
+      //pot, then will run the updatePID function to find the error difference and
+      //learn how to get good feedback with the PID controller
+      sPID mPid;
+      sPID* pmPid;
+      float currVal, prev_curr;
+//      float currFactor;
+      pmPid = &mPid;    // .1
+           //      P   , I, D, Imax, Imin, Dstate, Istate
+//      setPID(pmPid,0.07, 0, .6, 0.45, -0.1, 0, 0); //gains for when working with Degs
+      setPID(pmPid,0.9, 0, 7, 0.45, -0.1, 0, 0); //gains when working in radians
+    
+      float t, t_ref;
+
+      //initialize PID controller
+      motor.period(0.00005f);       //Set PWM frequency.  MC33926 h-bridge limited 20 kHz (.00005 seconds)
+
+      timer.start();
+      t_ref = 0;
+      while (1) {
+            //Ctrl pot range will be -30 to 30 degrees, just to have high point resolution
+            //Gather inputs
+            ctrlDeg = ctrlPot*(1.0472f)- 0.5236f;            
+            shaftDeg = pulsesToRadians(wheel.getPulses());
+            degErr = ctrlDeg - shaftDeg;
+            
+//            currFactor = currPot*3.3*(1/3.3);
+            currVal = Current.read()*3.3/.140f;
+            
+            
+            //Update PID vars and send CMD signal to motor
+            cmdSig = UpdatePID(pmPid, degErr, shaftDeg);
+            signal_to_hbridge(cmdSig);
+            t = timer.read();
+            //Print out result
+           // pc.printf("t: %010lu\t SP: %3.3f\t PV: %3.3f\t CV: %3.3f\n",
+//                      t, ctrlDeg, shaftDeg, cmdSig);
+            if (t - t_ref > 0.1) {
+                pc.printf("%6.3f\t %5.2f\t %3.3f\t %3.3f\t %3.3f\n",
+                  //t, ctrlDeg, shaftDeg, cmdSig);
+                   t, ctrlDeg, shaftDeg, degErr, currVal);
+                   t_ref = t;
+               //Current protection of motor
+               if (((currVal > 6.0f) &&  (abs(currVal - prev_curr) <0.20))) break;
+               
+               prev_curr = currVal;
+                      }//end if t_ref
+            wait(0.0001f);
+        }//end while loop
+        //assuming motor needs braknig
+        signal_to_hbridge(0);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Aug 02 14:46:49 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Aug 02 14:46:49 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6f327212ef96
\ No newline at end of file