Control function for hip motors
Revision 1:d87dac5c3658, committed 2015-06-24
- Comitter:
- perr1940
- Date:
- Wed Jun 24 01:07:55 2015 +0000
- Parent:
- 0:911517b34248
- Commit message:
- I don't know what I changed
Changed in this revision
HipControl.cpp | Show annotated file Show diff for this revision Revisions of this file |
HipControl.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 911517b34248 -r d87dac5c3658 HipControl.cpp --- 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;
diff -r 911517b34248 -r d87dac5c3658 HipControl.h --- a/HipControl.h Wed Nov 19 22:11:34 2014 +0000 +++ b/HipControl.h Wed Jun 24 01:07:55 2015 +0000 @@ -22,6 +22,8 @@ * @brief Control algorithms */ +//TODO: (Brad) Port to base-class structure + #include "mbed.h" #include "filter.h" @@ -29,18 +31,47 @@ { public: HipControl(PinName pwm, PinName dirpin); + + /** + * Feedback linearization and gain scheduling controller. Used for all hip trajectory following. + * @param ref Reference point to track. + * @param pos Current position in degrees + * @param Kp Proportional gain + * @param Kd Derivative gain + * @param sat Commanded current saturation + */ + //class void FL(float ref, float pos); - void FL_new(float ref, float pos); + + /** + * Vanilla PD controller for set-point tracking. Mostly used for haptics. + * @param ref Reference point to track. + * @param pos Current position in degrees + * @param Kp Proportional gain + * @param Kd Derivative gain + * @param sat Commanded current saturation + */ + //class void PD(float ref, float pos); +//class void P(float ref, float pos); + //base method void setGains(float P, float D); + //base method void setSat(float limit); + //base method void sampleTime(float time); + //class void openLoop(float input); - float read(); + //base method + float readPWM(); + //class void off(); + //base method void flip(); + //base method void clear(); + //base method void pwmPeriod(float a); private: //Controller Parameters @@ -48,31 +79,30 @@ PwmOut _pwm; DigitalOut _dir; float Kp; + /** + * Initial proportional gain before cosine gain schedule + */ const float Kp0; + /** + * Derivative gain + */ float Kd; + /** + * Commanded current saturation + */ float sat; float u; float u_prev; float error[2]; - float T; + //sample period + float _sample_period; int sign; filter controlFilter; }; //Controller Parameters -/** -* Proportional gain after cosine gain schedule -*/ -/** -* Initial proportional gain before cosine gain schedule -*/ -/** -* Derivative gain -*/ -/** -* Commanded current soft stop -*/ + /** * Counter for proportional gain cosine gain scheduling */ @@ -92,20 +122,5 @@ #endif -/** -* Feedback linearization and gain scheduling controller. Used for all hip trajectory following. -* @param ref Reference point to track. -* @param pos Current position in degrees -* @param Kp Proportional gain -* @param Kd Derivative gain -* @param sat Commanded current saturation -*/ -/** -* Vanilla PD controller for set-point tracking. Mostly used for haptics. -* @param ref Reference point to track. -* @param pos Current position in degrees -* @param Kp Proportional gain -* @param Kd Derivative gain -* @param sat Commanded current saturation -*/ \ No newline at end of file +