Nathaniel Honka / HipControl

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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 }