Corrected header file include guards.
Fork of HipControl by
HipControl.cpp@1:d87dac5c3658, 2015-06-24 (annotated)
- Committer:
- perr1940
- Date:
- Wed Jun 24 01:07:55 2015 +0000
- Revision:
- 1:d87dac5c3658
- Parent:
- 0:911517b34248
I don't know what I changed
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 | 1:d87dac5c3658 | 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), _sample_period(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 | 1:d87dac5c3658 | 22 | _sample_period=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 | 1:d87dac5c3658 | 36 | float HipControl::readPWM() |
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 | 1:d87dac5c3658 | 67 | u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/_sample_period+.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::PD(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 | |
perr1940 | 1:d87dac5c3658 | 92 | u = sign*controlFilter.Butterworth_1K(Kp*error[1]+Kd*(error[1]-error[0])/_sample_period); |
perr1940 | 0:911517b34248 | 93 | |
perr1940 | 0:911517b34248 | 94 | if (u > 0) { |
perr1940 | 0:911517b34248 | 95 | _dir = 1; |
perr1940 | 0:911517b34248 | 96 | } else { |
perr1940 | 0:911517b34248 | 97 | _dir = 0; |
perr1940 | 0:911517b34248 | 98 | } |
perr1940 | 0:911517b34248 | 99 | |
perr1940 | 0:911517b34248 | 100 | if (u > sat) { |
perr1940 | 0:911517b34248 | 101 | u = sat; |
perr1940 | 0:911517b34248 | 102 | } else if (u < -sat) { |
perr1940 | 0:911517b34248 | 103 | u = -sat; |
perr1940 | 0:911517b34248 | 104 | } |
perr1940 | 0:911517b34248 | 105 | u_prev=u; |
perr1940 | 0:911517b34248 | 106 | if (u < 0) { |
perr1940 | 0:911517b34248 | 107 | u = -u; |
perr1940 | 0:911517b34248 | 108 | } |
perr1940 | 0:911517b34248 | 109 | _pwm = u; |
perr1940 | 0:911517b34248 | 110 | } |
perr1940 | 0:911517b34248 | 111 | |
perr1940 | 0:911517b34248 | 112 | void HipControl::P(float ref, float pos) |
perr1940 | 0:911517b34248 | 113 | { |
perr1940 | 0:911517b34248 | 114 | error[0]=error[1]; |
perr1940 | 0:911517b34248 | 115 | error[1] = ref - pos; |
perr1940 | 0:911517b34248 | 116 | |
perr1940 | 0:911517b34248 | 117 | u = sign*Kp*error[1]; |
perr1940 | 0:911517b34248 | 118 | float temp=controlFilter.Butterworth_1K(u); |
perr1940 | 0:911517b34248 | 119 | |
perr1940 | 0:911517b34248 | 120 | if (u > 0) { |
perr1940 | 0:911517b34248 | 121 | _dir = 1; |
perr1940 | 0:911517b34248 | 122 | } else { |
perr1940 | 0:911517b34248 | 123 | _dir = 0; |
perr1940 | 0:911517b34248 | 124 | } |
perr1940 | 0:911517b34248 | 125 | |
perr1940 | 0:911517b34248 | 126 | if (u > sat) { |
perr1940 | 0:911517b34248 | 127 | u = sat; |
perr1940 | 0:911517b34248 | 128 | } else if (u < -sat) { |
perr1940 | 0:911517b34248 | 129 | u = -sat; |
perr1940 | 0:911517b34248 | 130 | } |
perr1940 | 0:911517b34248 | 131 | u_prev=u; |
perr1940 | 0:911517b34248 | 132 | if (u < 0) { |
perr1940 | 0:911517b34248 | 133 | u = -u; |
perr1940 | 0:911517b34248 | 134 | } |
perr1940 | 0:911517b34248 | 135 | _pwm = u; |
perr1940 | 0:911517b34248 | 136 | } |
perr1940 | 0:911517b34248 | 137 | void HipControl::off() |
perr1940 | 0:911517b34248 | 138 | { |
perr1940 | 0:911517b34248 | 139 | _pwm=0; |
perr1940 | 0:911517b34248 | 140 | } |
perr1940 | 0:911517b34248 | 141 | |
perr1940 | 0:911517b34248 | 142 | void HipControl::flip() |
perr1940 | 0:911517b34248 | 143 | { |
perr1940 | 0:911517b34248 | 144 | sign=-sign; |
perr1940 | 0:911517b34248 | 145 | } |