Corrected header file include guards.
Fork of HipControl by
Diff: HipControl.cpp
- Revision:
- 0:911517b34248
- Child:
- 1:d87dac5c3658
diff -r 000000000000 -r 911517b34248 HipControl.cpp --- /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