Nathaniel Honka / HipControl

Corrected header file include guards.

Fork of HipControl by Bradley Perry

HipControl.cpp@2:18e264a5b71d, 2015-07-02 (annotated)

Committer:
nathanhonka
Date:
Thu Jul 02 21:36:58 2015 +0000
Revision:
2:18e264a5b71d
Parent:
1:d87dac5c3658 
Corrected header file include guards (did not encapsulate entire header).

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