HipControl.cpp

00001 #include "mbed.h"
00002 #include "filter.h"
00003 #include "HipControl.h"
00004 
00005 #define TO_RAD(x) (x * 0.01745329252)  // *pi/180
00006 
00007 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)
00008 {
00009     _pwm.period(.00005);
00010     _pwm=0;
00011     _dir=0;
00012 }
00013 void HipControl::clear(){
00014     error[1]=error[0]=0;
00015     }
00016 void HipControl::pwmPeriod(float a)
00017 {
00018     _pwm.period(a);
00019 }
00020 void HipControl::sampleTime(float time)
00021 {
00022     _sample_period=time;
00023 }
00024 void HipControl::setGains(float P, float D)
00025 {
00026     Kp=P;
00027     Kd=D;
00028 }
00029 
00030 void HipControl::setSat(float limit)
00031 {
00032     sat=limit;
00033 }
00034 
00035 
00036 float HipControl::readPWM()
00037 {
00038     return u_prev;
00039 }
00040 
00041 void HipControl::openLoop(float input)
00042 {
00043     u=sign*input;
00044     if (u > 0) {
00045         _dir = 1;
00046     } else {
00047         _dir = 0;
00048     }
00049 
00050     if (u > sat) {
00051         u = sat;
00052     } else if (u < -sat) {
00053         u = -sat;
00054     }
00055     u_prev=u;
00056     if (u < 0) {
00057         u = -u;
00058     }
00059     _pwm = u;
00060 }
00061 
00062 void HipControl::FL(float ref, float pos)
00063 {
00064     error[0]=error[1];
00065     error[1] = ref - pos;
00066 
00067     u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/_sample_period+.173*sin(TO_RAD(pos)));
00068 
00069     if (u > 0) {
00070         _dir = 1;
00071     } else {
00072         _dir = 0;
00073     }
00074 
00075     if (u > sat) {
00076         u = sat;
00077     } else if (u < -sat) {
00078         u = -sat;
00079     }
00080     u_prev=u;
00081     if (u < 0) {
00082         u = -u;
00083     }
00084     _pwm = u;
00085 }
00086 
00087 void HipControl::PD(float ref, float pos)
00088 {
00089     error[0]=error[1];
00090     error[1] = ref - pos;
00091 
00092     u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/_sample_period);
00093 
00094     if (u > 0) {
00095         _dir = 1;
00096     } else {
00097         _dir = 0;
00098     }
00099 
00100     if (u > sat) {
00101         u = sat;
00102     } else if (u < -sat) {
00103         u = -sat;
00104     }
00105     u_prev=u;
00106     if (u < 0) {
00107         u = -u;
00108     }
00109     _pwm = u;
00110 }
00111 
00112 void HipControl::P(float ref, float pos)
00113 {
00114     error[0]=error[1];
00115     error[1] = ref - pos;
00116 
00117     u = sign*Kp*error[1];
00118     float temp=controlFilter.Butterworth_1K(u);
00119 
00120     if (u > 0) {
00121         _dir = 1;
00122     } else {
00123         _dir = 0;
00124     }
00125 
00126     if (u > sat) {
00127         u = sat;
00128     } else if (u < -sat) {
00129         u = -sat;
00130     }
00131     u_prev=u;
00132     if (u < 0) {
00133         u = -u;
00134     }
00135     _pwm = u;
00136 }
00137 void HipControl::off()
00138 {
00139     _pwm=0;
00140 }
00141 
00142 void HipControl::flip()
00143 {
00144     sign=-sign;
00145 }