Control function for hip motors

Files at this revision

API Documentation at this revision

Comitter:
perr1940
Date:
Wed Jun 24 01:07:55 2015 +0000
Parent:
0:911517b34248
Commit message:
I don't know what I changed

Changed in this revision

HipControl.cpp Show annotated file Show diff for this revision Revisions of this file
HipControl.h Show annotated file Show diff for this revision Revisions of this file
diff -r 911517b34248 -r d87dac5c3658 HipControl.cpp
--- a/HipControl.cpp	Wed Nov 19 22:11:34 2014 +0000
+++ b/HipControl.cpp	Wed Jun 24 01:07:55 2015 +0000
@@ -4,7 +4,7 @@
 
 #define TO_RAD(x) (x * 0.01745329252)  // *pi/180
 
-HipControl::HipControl(PinName pwm, PinName dirpin): _pwm(pwm), _dir(dirpin), Kp(0.02), Kd(0.002), sat(0.4), Kp0(0.001), u_prev(0), T(0.001), sign(1)
+HipControl::HipControl(PinName pwm, PinName dirpin): _pwm(pwm), _dir(dirpin), Kp(0.02), Kd(0.002), sat(0.4), Kp0(0.001), u_prev(0), _sample_period(0.001), sign(1)
 {
     _pwm.period(.00005);
     _pwm=0;
@@ -19,7 +19,7 @@
 }
 void HipControl::sampleTime(float time)
 {
-    T=time;
+    _sample_period=time;
 }
 void HipControl::setGains(float P, float D)
 {
@@ -33,7 +33,7 @@
 }
 
 
-float HipControl::read()
+float HipControl::readPWM()
 {
     return u_prev;
 }
@@ -64,7 +64,7 @@
     error[0]=error[1];
     error[1] = ref - pos;
 
-    u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/T+.173*sin(TO_RAD(pos)));
+    u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/_sample_period+.173*sin(TO_RAD(pos)));
 
     if (u > 0) {
         _dir = 1;
@@ -84,42 +84,12 @@
     _pwm = u;
 }
 
-void HipControl::FL_new(float ref, float pos)
-{
-    error[0]=error[1];
-    error[1] = ref - pos;
-    float blah=Kd/3*2/T+1;
-    float alpha=(Kp+Kp*Kd/3*2/T+Kd*2/T)/blah;
-    float beta=(Kp-Kp*Kd/3*2/T-Kd*2/T)/blah;
-    float gamma=(Kd/3*2/T+1)/blah;
-
-    u = sign*controlFilter.Butterworth_1K(alpha*error[1]+beta*error[0]-gamma*u_prev+.173*sin(TO_RAD(pos)));
-
-    if (u > 0) {
-        _dir = 1;
-    } else {
-        _dir = 0;
-    }
-
-    if (u > sat) {
-        u = sat;
-    } else if (u < -sat) {
-        u = -sat;
-    }
-    u_prev=u;
-
-    if (u < 0) {
-        u = -u;
-    }
-    _pwm = u;
-}
-
 void HipControl::PD(float ref, float pos)
 {
     error[0]=error[1];
     error[1] = ref - pos;
 
-    u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/T);
+    u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/_sample_period);
 
     if (u > 0) {
         _dir = 1;
diff -r 911517b34248 -r d87dac5c3658 HipControl.h
--- a/HipControl.h	Wed Nov 19 22:11:34 2014 +0000
+++ b/HipControl.h	Wed Jun 24 01:07:55 2015 +0000
@@ -22,6 +22,8 @@
 * @brief Control algorithms
 */
 
+//TODO: (Brad) Port to base-class structure
+
 #include "mbed.h"
 #include "filter.h"
 
@@ -29,18 +31,47 @@
 {
 public:
     HipControl(PinName pwm, PinName dirpin);
+
+    /**
+    * Feedback linearization and gain scheduling controller.  Used for all hip trajectory following.
+    * @param ref Reference point to track.
+    * @param pos Current position in degrees
+    * @param Kp Proportional gain
+    * @param Kd Derivative gain
+    * @param sat Commanded current saturation
+    */
+    //class
     void FL(float ref, float pos);
-    void FL_new(float ref, float pos);
+
+    /**
+    * Vanilla PD controller for set-point tracking.  Mostly used for haptics.
+    * @param ref Reference point to track.
+    * @param pos Current position in degrees
+    * @param Kp Proportional gain
+    * @param Kd Derivative gain
+    * @param sat Commanded current saturation
+    */
+    //class
     void PD(float ref, float pos);
+//class
     void P(float ref, float pos);
+    //base method
     void setGains(float P, float D);
+    //base method
     void setSat(float limit);
+    //base method
     void sampleTime(float time);
+    //class
     void openLoop(float input);
-    float read();
+    //base method
+    float readPWM();
+    //class
     void off();
+    //base method
     void flip();
+    //base method
     void clear();
+    //base method
     void pwmPeriod(float a);
 private:
     //Controller Parameters
@@ -48,31 +79,30 @@
     PwmOut _pwm;
     DigitalOut _dir;
     float Kp;
+    /**
+    * Initial proportional gain before cosine gain schedule
+    */
     const float Kp0;
+    /**
+    * Derivative gain
+    */
     float Kd;
+    /**
+    * Commanded current saturation
+    */
     float sat;
     float u;
     float u_prev;
     float error[2];
-    float T;
+    //sample period
+    float _sample_period;
     int sign;
     filter controlFilter;
 };
 
 //Controller Parameters
-/**
-* Proportional gain after cosine gain schedule
-*/
-/**
-* Initial proportional gain before cosine gain schedule
-*/
-/**
-* Derivative gain
-*/
 
-/**
-* Commanded current soft stop
-*/
+
 /**
 * Counter for proportional gain cosine gain scheduling
 */
@@ -92,20 +122,5 @@
 
 #endif
 
-/**
-* Feedback linearization and gain scheduling controller.  Used for all hip trajectory following.
-* @param ref Reference point to track.
-* @param pos Current position in degrees
-* @param Kp Proportional gain
-* @param Kd Derivative gain
-* @param sat Commanded current saturation
-*/
 
-/**
-* Vanilla PD controller for set-point tracking.  Mostly used for haptics.
-* @param ref Reference point to track.
-* @param pos Current position in degrees
-* @param Kp Proportional gain
-* @param Kd Derivative gain
-* @param sat Commanded current saturation
-*/
\ No newline at end of file
+