Corrected header file include guards.
Fork of HipControl by
HipControl.cpp
- Committer:
- perr1940
- Date:
- 2014-11-19
- Revision:
- 0:911517b34248
- Child:
- 1:d87dac5c3658
File content as of revision 0:911517b34248:
#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; }