Diff: HipControl.cpp

Revision:

1:d87dac5c3658

Parent:

0:911517b34248

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