Corrected header file include guards.
Fork of HipControl by
HipControl.cpp
- Committer:
- nathanhonka
- Date:
- 2015-07-02
- Revision:
- 2:18e264a5b71d
- Parent:
- 1:d87dac5c3658
File content as of revision 2:18e264a5b71d:
#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), _sample_period(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) { _sample_period=time; } void HipControl::setGains(float P, float D) { Kp=P; Kd=D; } void HipControl::setSat(float limit) { sat=limit; } float HipControl::readPWM() { 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])/_sample_period+.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])/_sample_period); 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; }