Diff: HipControl.cpp

Revision:

0:911517b34248

Child:

1:d87dac5c3658

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HipControl.cpp	Wed Nov 19 22:11:34 2014 +0000
@@ -0,0 +1,175 @@
+#include "mbed.h"
+#include "filter.h"
+#include "HipControl.h"
+
+#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)
+{
+    _pwm.period(.00005);
+    _pwm=0;
+    _dir=0;
+}
+void HipControl::clear(){
+    error[1]=error[0]=0;
+    }
+void HipControl::pwmPeriod(float a)
+{
+    _pwm.period(a);
+}
+void HipControl::sampleTime(float time)
+{
+    T=time;
+}
+void HipControl::setGains(float P, float D)
+{
+    Kp=P;
+    Kd=D;
+}
+
+void HipControl::setSat(float limit)
+{
+    sat=limit;
+}
+
+
+float HipControl::read()
+{
+    return u_prev;
+}
+
+void HipControl::openLoop(float input)
+{
+    u=sign*input;
+    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::FL(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+.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::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);
+
+    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::P(float ref, float pos)
+{
+    error[0]=error[1];
+    error[1] = ref - pos;
+
+    u = sign*Kp*error[1];
+    float temp=controlFilter.Butterworth_1K(u);
+
+    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::off()
+{
+    _pwm=0;
+}
+
+void HipControl::flip()
+{
+    sign=-sign;
+}
\ No newline at end of file