Nathaniel Honka / HipControl

Corrected header file include guards.

Fork of HipControl by Bradley Perry

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?

UserRevisionLine numberNew 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 }