Control function for hip motors
HipControl.cpp@0:911517b34248, 2014-11-19 (annotated)
- Committer:
- perr1940
- Date:
- Wed Nov 19 22:11:34 2014 +0000
- Revision:
- 0:911517b34248
- Child:
- 1:d87dac5c3658
Sharing with Michael Ling
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
perr1940 | 0:911517b34248 | 1 | #include "mbed.h" |
perr1940 | 0:911517b34248 | 2 | #include "filter.h" |
perr1940 | 0:911517b34248 | 3 | #include "HipControl.h" |
perr1940 | 0:911517b34248 | 4 | |
perr1940 | 0:911517b34248 | 5 | #define TO_RAD(x) (x * 0.01745329252) // *pi/180 |
perr1940 | 0:911517b34248 | 6 | |
perr1940 | 0:911517b34248 | 7 | 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) |
perr1940 | 0:911517b34248 | 8 | { |
perr1940 | 0:911517b34248 | 9 | _pwm.period(.00005); |
perr1940 | 0:911517b34248 | 10 | _pwm=0; |
perr1940 | 0:911517b34248 | 11 | _dir=0; |
perr1940 | 0:911517b34248 | 12 | } |
perr1940 | 0:911517b34248 | 13 | void HipControl::clear(){ |
perr1940 | 0:911517b34248 | 14 | error[1]=error[0]=0; |
perr1940 | 0:911517b34248 | 15 | } |
perr1940 | 0:911517b34248 | 16 | void HipControl::pwmPeriod(float a) |
perr1940 | 0:911517b34248 | 17 | { |
perr1940 | 0:911517b34248 | 18 | _pwm.period(a); |
perr1940 | 0:911517b34248 | 19 | } |
perr1940 | 0:911517b34248 | 20 | void HipControl::sampleTime(float time) |
perr1940 | 0:911517b34248 | 21 | { |
perr1940 | 0:911517b34248 | 22 | T=time; |
perr1940 | 0:911517b34248 | 23 | } |
perr1940 | 0:911517b34248 | 24 | void HipControl::setGains(float P, float D) |
perr1940 | 0:911517b34248 | 25 | { |
perr1940 | 0:911517b34248 | 26 | Kp=P; |
perr1940 | 0:911517b34248 | 27 | Kd=D; |
perr1940 | 0:911517b34248 | 28 | } |
perr1940 | 0:911517b34248 | 29 | |
perr1940 | 0:911517b34248 | 30 | void HipControl::setSat(float limit) |
perr1940 | 0:911517b34248 | 31 | { |
perr1940 | 0:911517b34248 | 32 | sat=limit; |
perr1940 | 0:911517b34248 | 33 | } |
perr1940 | 0:911517b34248 | 34 | |
perr1940 | 0:911517b34248 | 35 | |
perr1940 | 0:911517b34248 | 36 | float HipControl::read() |
perr1940 | 0:911517b34248 | 37 | { |
perr1940 | 0:911517b34248 | 38 | return u_prev; |
perr1940 | 0:911517b34248 | 39 | } |
perr1940 | 0:911517b34248 | 40 | |
perr1940 | 0:911517b34248 | 41 | void HipControl::openLoop(float input) |
perr1940 | 0:911517b34248 | 42 | { |
perr1940 | 0:911517b34248 | 43 | u=sign*input; |
perr1940 | 0:911517b34248 | 44 | if (u > 0) { |
perr1940 | 0:911517b34248 | 45 | _dir = 1; |
perr1940 | 0:911517b34248 | 46 | } else { |
perr1940 | 0:911517b34248 | 47 | _dir = 0; |
perr1940 | 0:911517b34248 | 48 | } |
perr1940 | 0:911517b34248 | 49 | |
perr1940 | 0:911517b34248 | 50 | if (u > sat) { |
perr1940 | 0:911517b34248 | 51 | u = sat; |
perr1940 | 0:911517b34248 | 52 | } else if (u < -sat) { |
perr1940 | 0:911517b34248 | 53 | u = -sat; |
perr1940 | 0:911517b34248 | 54 | } |
perr1940 | 0:911517b34248 | 55 | u_prev=u; |
perr1940 | 0:911517b34248 | 56 | if (u < 0) { |
perr1940 | 0:911517b34248 | 57 | u = -u; |
perr1940 | 0:911517b34248 | 58 | } |
perr1940 | 0:911517b34248 | 59 | _pwm = u; |
perr1940 | 0:911517b34248 | 60 | } |
perr1940 | 0:911517b34248 | 61 | |
perr1940 | 0:911517b34248 | 62 | void HipControl::FL(float ref, float pos) |
perr1940 | 0:911517b34248 | 63 | { |
perr1940 | 0:911517b34248 | 64 | error[0]=error[1]; |
perr1940 | 0:911517b34248 | 65 | error[1] = ref - pos; |
perr1940 | 0:911517b34248 | 66 | |
perr1940 | 0:911517b34248 | 67 | u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/T+.173*sin(TO_RAD(pos))); |
perr1940 | 0:911517b34248 | 68 | |
perr1940 | 0:911517b34248 | 69 | if (u > 0) { |
perr1940 | 0:911517b34248 | 70 | _dir = 1; |
perr1940 | 0:911517b34248 | 71 | } else { |
perr1940 | 0:911517b34248 | 72 | _dir = 0; |
perr1940 | 0:911517b34248 | 73 | } |
perr1940 | 0:911517b34248 | 74 | |
perr1940 | 0:911517b34248 | 75 | if (u > sat) { |
perr1940 | 0:911517b34248 | 76 | u = sat; |
perr1940 | 0:911517b34248 | 77 | } else if (u < -sat) { |
perr1940 | 0:911517b34248 | 78 | u = -sat; |
perr1940 | 0:911517b34248 | 79 | } |
perr1940 | 0:911517b34248 | 80 | u_prev=u; |
perr1940 | 0:911517b34248 | 81 | if (u < 0) { |
perr1940 | 0:911517b34248 | 82 | u = -u; |
perr1940 | 0:911517b34248 | 83 | } |
perr1940 | 0:911517b34248 | 84 | _pwm = u; |
perr1940 | 0:911517b34248 | 85 | } |
perr1940 | 0:911517b34248 | 86 | |
perr1940 | 0:911517b34248 | 87 | void HipControl::FL_new(float ref, float pos) |
perr1940 | 0:911517b34248 | 88 | { |
perr1940 | 0:911517b34248 | 89 | error[0]=error[1]; |
perr1940 | 0:911517b34248 | 90 | error[1] = ref - pos; |
perr1940 | 0:911517b34248 | 91 | float blah=Kd/3*2/T+1; |
perr1940 | 0:911517b34248 | 92 | float alpha=(Kp+Kp*Kd/3*2/T+Kd*2/T)/blah; |
perr1940 | 0:911517b34248 | 93 | float beta=(Kp-Kp*Kd/3*2/T-Kd*2/T)/blah; |
perr1940 | 0:911517b34248 | 94 | float gamma=(Kd/3*2/T+1)/blah; |
perr1940 | 0:911517b34248 | 95 | |
perr1940 | 0:911517b34248 | 96 | u = sign*controlFilter.Butterworth_1K(alpha*error[1]+beta*error[0]-gamma*u_prev+.173*sin(TO_RAD(pos))); |
perr1940 | 0:911517b34248 | 97 | |
perr1940 | 0:911517b34248 | 98 | if (u > 0) { |
perr1940 | 0:911517b34248 | 99 | _dir = 1; |
perr1940 | 0:911517b34248 | 100 | } else { |
perr1940 | 0:911517b34248 | 101 | _dir = 0; |
perr1940 | 0:911517b34248 | 102 | } |
perr1940 | 0:911517b34248 | 103 | |
perr1940 | 0:911517b34248 | 104 | if (u > sat) { |
perr1940 | 0:911517b34248 | 105 | u = sat; |
perr1940 | 0:911517b34248 | 106 | } else if (u < -sat) { |
perr1940 | 0:911517b34248 | 107 | u = -sat; |
perr1940 | 0:911517b34248 | 108 | } |
perr1940 | 0:911517b34248 | 109 | u_prev=u; |
perr1940 | 0:911517b34248 | 110 | |
perr1940 | 0:911517b34248 | 111 | if (u < 0) { |
perr1940 | 0:911517b34248 | 112 | u = -u; |
perr1940 | 0:911517b34248 | 113 | } |
perr1940 | 0:911517b34248 | 114 | _pwm = u; |
perr1940 | 0:911517b34248 | 115 | } |
perr1940 | 0:911517b34248 | 116 | |
perr1940 | 0:911517b34248 | 117 | void HipControl::PD(float ref, float pos) |
perr1940 | 0:911517b34248 | 118 | { |
perr1940 | 0:911517b34248 | 119 | error[0]=error[1]; |
perr1940 | 0:911517b34248 | 120 | error[1] = ref - pos; |
perr1940 | 0:911517b34248 | 121 | |
perr1940 | 0:911517b34248 | 122 | u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/T); |
perr1940 | 0:911517b34248 | 123 | |
perr1940 | 0:911517b34248 | 124 | if (u > 0) { |
perr1940 | 0:911517b34248 | 125 | _dir = 1; |
perr1940 | 0:911517b34248 | 126 | } else { |
perr1940 | 0:911517b34248 | 127 | _dir = 0; |
perr1940 | 0:911517b34248 | 128 | } |
perr1940 | 0:911517b34248 | 129 | |
perr1940 | 0:911517b34248 | 130 | if (u > sat) { |
perr1940 | 0:911517b34248 | 131 | u = sat; |
perr1940 | 0:911517b34248 | 132 | } else if (u < -sat) { |
perr1940 | 0:911517b34248 | 133 | u = -sat; |
perr1940 | 0:911517b34248 | 134 | } |
perr1940 | 0:911517b34248 | 135 | u_prev=u; |
perr1940 | 0:911517b34248 | 136 | if (u < 0) { |
perr1940 | 0:911517b34248 | 137 | u = -u; |
perr1940 | 0:911517b34248 | 138 | } |
perr1940 | 0:911517b34248 | 139 | _pwm = u; |
perr1940 | 0:911517b34248 | 140 | } |
perr1940 | 0:911517b34248 | 141 | |
perr1940 | 0:911517b34248 | 142 | void HipControl::P(float ref, float pos) |
perr1940 | 0:911517b34248 | 143 | { |
perr1940 | 0:911517b34248 | 144 | error[0]=error[1]; |
perr1940 | 0:911517b34248 | 145 | error[1] = ref - pos; |
perr1940 | 0:911517b34248 | 146 | |
perr1940 | 0:911517b34248 | 147 | u = sign*Kp*error[1]; |
perr1940 | 0:911517b34248 | 148 | float temp=controlFilter.Butterworth_1K(u); |
perr1940 | 0:911517b34248 | 149 | |
perr1940 | 0:911517b34248 | 150 | if (u > 0) { |
perr1940 | 0:911517b34248 | 151 | _dir = 1; |
perr1940 | 0:911517b34248 | 152 | } else { |
perr1940 | 0:911517b34248 | 153 | _dir = 0; |
perr1940 | 0:911517b34248 | 154 | } |
perr1940 | 0:911517b34248 | 155 | |
perr1940 | 0:911517b34248 | 156 | if (u > sat) { |
perr1940 | 0:911517b34248 | 157 | u = sat; |
perr1940 | 0:911517b34248 | 158 | } else if (u < -sat) { |
perr1940 | 0:911517b34248 | 159 | u = -sat; |
perr1940 | 0:911517b34248 | 160 | } |
perr1940 | 0:911517b34248 | 161 | u_prev=u; |
perr1940 | 0:911517b34248 | 162 | if (u < 0) { |
perr1940 | 0:911517b34248 | 163 | u = -u; |
perr1940 | 0:911517b34248 | 164 | } |
perr1940 | 0:911517b34248 | 165 | _pwm = u; |
perr1940 | 0:911517b34248 | 166 | } |
perr1940 | 0:911517b34248 | 167 | void HipControl::off() |
perr1940 | 0:911517b34248 | 168 | { |
perr1940 | 0:911517b34248 | 169 | _pwm=0; |
perr1940 | 0:911517b34248 | 170 | } |
perr1940 | 0:911517b34248 | 171 | |
perr1940 | 0:911517b34248 | 172 | void HipControl::flip() |
perr1940 | 0:911517b34248 | 173 | { |
perr1940 | 0:911517b34248 | 174 | sign=-sign; |
perr1940 | 0:911517b34248 | 175 | } |