Control function for hip motors
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 15 2022 01:49:01 by 1.7.2