Revision 1:d87dac5c3658, committed 2015-06-24

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

--- 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;

--- 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
+