Menno Sytsma / Actief_stuurprogramma

Dit is het actieve stuurprogramma, behorend bij de bacheloropdracht van Menno Sytsma over de integratie van een Twisted string actuator in een handorthese

Dependencies:   EMG FastPWM HIDScope mbed-src

main.cpp@7:2bbd0147b07b, 2016-06-22 (annotated)

Committer:
s1503189
Date:
Wed Jun 22 07:38:49 2016 +0000
Revision:
7:2bbd0147b07b
Parent:
6:35d5bab53f44 
Gepubliceerde versie;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1503189 0:c727f4699e80 1 #include "mbed.h"
s1503189 0:c727f4699e80 2 #include "HIDScope.h"
s1503189 0:c727f4699e80 3 #include "biquadFilter.h"
s1503189 0:c727f4699e80 4 #include "FastPWM.h"
s1503189 0:c727f4699e80 5
s1503189 0:c727f4699e80 6 // Define the HIDScope and Ticker object
s1503189 0:c727f4699e80 7 HIDScope scope(5);
s1503189 0:c727f4699e80 8 Ticker scopeTimer;
s1503189 0:c727f4699e80 9 volatile bool LoopTimerFlag;
s1503189 0:c727f4699e80 10 Ticker ForceTimer;
s1503189 0:c727f4699e80 11 volatile bool LoopTimerFlag2;
s1503189 0:c727f4699e80 12
s1503189 0:c727f4699e80 13 // Read the analog input
s1503189 0:c727f4699e80 14 AnalogIn a_in2(A3); //OUT-
s1503189 0:c727f4699e80 15 AnalogIn a_in3(A4); //OUT+
s1503189 0:c727f4699e80 16
s1503189 0:c727f4699e80 17 Ticker Loopticker; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
s1503189 0:c727f4699e80 18 AnalogIn Referentie(A0); // De schuifpotmeter naast de geleiding van de draad is de referentie.
s1503189 0:c727f4699e80 19 AnalogIn Boardpotmeter2(A2);
s1503189 0:c727f4699e80 20 DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
s1503189 0:c727f4699e80 21 FastPWM motor1speed(D6);
s1503189 0:c727f4699e80 22 DigitalOut Led(LED_RED);
s1503189 0:c727f4699e80 23 DigitalOut Roodisplus(D5);
s1503189 0:c727f4699e80 24 DigitalOut Versterker(D4);
s1503189 1:57926254e65b 25
s1503189 0:c727f4699e80 26 float Error;
s1503189 0:c727f4699e80 27 float Referentie2;//=1.0;
s1503189 0:c727f4699e80 28 float Output;
s1503189 0:c727f4699e80 29 float Input;
s1503189 0:c727f4699e80 30 double Error_prev = 0;
s1503189 0:c727f4699e80 31 double Error_int = 0;
s1503189 0:c727f4699e80 32 double Error_der;
s1503189 0:c727f4699e80 33 double Ref_der = 0;
s1503189 0:c727f4699e80 34 double Ref_prev = 0;
s1503189 0:c727f4699e80 35 double Ref_prev2 = 0;
s1503189 0:c727f4699e80 36 double Ref2;
s1503189 0:c727f4699e80 37 double Input3;
s1503189 0:c727f4699e80 38 double Plaats_der;
s1503189 0:c727f4699e80 39 double Force_nieuw=0;
s1503189 0:c727f4699e80 40 double Ref_der_prev;
s1503189 0:c727f4699e80 41 double Plaats_der1;
s1503189 0:c727f4699e80 42
s1503189 0:c727f4699e80 43 //LowPass filter 2 Hz
s1503189 7:2bbd0147b07b 44 const double a1_LP = -1.940778135263835, a2_LP = 0.942482022027066; // Biquad 300 Hz. fk = 2
s1503189 0:c727f4699e80 45 const double b0_LP = 0.000425971690807714, b1_LP = 0.000851943381615428, b2_LP = 0.000425971690807714;
s1503189 7:2bbd0147b07b 46 const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125; // fk = 0.5;
s1503189 0:c727f4699e80 47 const double b0_HP = 0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;
s1503189 1:57926254e65b 48 const double a1_LP5 = -1.822694925196308, a2_LP5 = 0.837181651256023 ;
s1503189 7:2bbd0147b07b 49 const double b0_LP5 = 0.003621681514929 , b1_LP5 = 0.007243363029857, b2_LP5 = 0.003621681514929; //Laagdoorlaatfilter, 100 Hz fK = 2
s1503189 7:2bbd0147b07b 50 const double a1_LP2 = -1.561018075800718, a2_LP2 = 0.641351538057563; // Laagdoorlaatfilter 300 Hz, fK = 15
s1503189 0:c727f4699e80 51 const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211;
s1503189 1:57926254e65b 52
s1503189 0:c727f4699e80 53
s1503189 1:57926254e65b 54 biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); // Lowpassfilter voor signaal van krachtsensor, 300 Hz, fk = 2
s1503189 1:57926254e65b 55 biquadFilter Filter2(a1_HP, a2_HP, b0_HP, b1_HP, b2_HP); // Highpassfilter voor signaal van krachtsensor, 300 Hz, fk = 10
s1503189 7:2bbd0147b07b 56 biquadFilter Filter3(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2); // Lowpassfilter voor signaal positie-potmeter, 300 Hz fK = 15
s1503189 7:2bbd0147b07b 57 biquadFilter Filter4(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2); // Lowpassfilter voor geschaalde signaal positie-potmeter, 300 Hz fK = 15
s1503189 1:57926254e65b 58 biquadFilter Filter6(a1_LP5, a2_LP5, b0_LP5, b1_LP5, b2_LP5); // Laagdoorlaatfilter 100 Hz, fk is 2 Hz
s1503189 1:57926254e65b 59
s1503189 0:c727f4699e80 60
s1503189 0:c727f4699e80 61 double Y_filt;
s1503189 0:c727f4699e80 62 double Y_filt2;
s1503189 0:c727f4699e80 63 double Y_filtprev;
s1503189 0:c727f4699e80 64 double Y_filtder;
s1503189 0:c727f4699e80 65 int A;
s1503189 0:c727f4699e80 66 float time_;
s1503189 0:c727f4699e80 67 const float time_increment = 0.005;
s1503189 0:c727f4699e80 68
s1503189 0:c727f4699e80 69 // The data read and send function
s1503189 0:c727f4699e80 70 void scopeSend()
s1503189 0:c727f4699e80 71 {
s1503189 5:d1c6f7fef412 72 scope.set(0,Ref2); // Force_nieuw);
s1503189 0:c727f4699e80 73 scope.set(1,Y_filtder);
s1503189 5:d1c6f7fef412 74 scope.set(2,Output); //Plaats_der);
s1503189 2:f5bb7fcfc7a2 75 scope.set(3,Input);
s1503189 0:c727f4699e80 76 scope.set(4,A);
s1503189 0:c727f4699e80 77 scope.send();
s1503189 0:c727f4699e80 78
s1503189 0:c727f4699e80 79 }
s1503189 0:c727f4699e80 80
s1503189 0:c727f4699e80 81 void tickerfunctie() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
s1503189 0:c727f4699e80 82 {
s1503189 0:c727f4699e80 83 LoopTimerFlag = 1;
s1503189 0:c727f4699e80 84 }
s1503189 0:c727f4699e80 85
s1503189 0:c727f4699e80 86 void tickerfunctie2() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
s1503189 0:c727f4699e80 87 {
s1503189 0:c727f4699e80 88 LoopTimerFlag2 = 1;
s1503189 0:c727f4699e80 89 }
s1503189 0:c727f4699e80 90
s1503189 0:c727f4699e80 91
s1503189 0:c727f4699e80 92 double Referentieschaling(double A, double B)
s1503189 0:c727f4699e80 93 {
s1503189 0:c727f4699e80 94 double Y= 787.3008916207206*pow(A,4) -565.1143141517078*pow(A,3) + 122.8516837382677*pow(A,2) + 0.0556616744031*A + 0.0912411880277;
s1503189 0:c727f4699e80 95 Referentie2 = Filter4.step(Y);
s1503189 5:d1c6f7fef412 96 if (Referentie2<=2.5 or Referentie2>=9.2) {
s1503189 0:c727f4699e80 97 Led = 0;
s1503189 0:c727f4699e80 98 Referentie2 = B;
s1503189 0:c727f4699e80 99 }
s1503189 0:c727f4699e80 100 return Referentie2;
s1503189 0:c727f4699e80 101 }
s1503189 0:c727f4699e80 102
s1503189 0:c727f4699e80 103 double Inputberekening(double B)
s1503189 0:c727f4699e80 104 {
s1503189 0:c727f4699e80 105 double Inp = 10*B; // De potmeter geeft ook waardes tussen 0 en 1, dit wordt met een factor 10 geschaald zodat deze als een positie in cm opgelegd kunnen worden.
s1503189 0:c727f4699e80 106 Input = Filter3.step(Inp);
s1503189 0:c727f4699e80 107 if (Input>=9) { // De Input moet binnnen een aantal grenzen blijven. Groter dan 7 is mechanisch niet mogelijk.
s1503189 0:c727f4699e80 108 Input=9; // Bij een waarde kleiner dan 1,5 zijn de strings niet meer gewikkeld en werkt de controller averechts en is deze uiterst instabiel.
s1503189 0:c727f4699e80 109 } else if (Input<=3.0) {
s1503189 0:c727f4699e80 110 Input = 3.0;
s1503189 0:c727f4699e80 111 }
s1503189 0:c727f4699e80 112 return Input;
s1503189 0:c727f4699e80 113 }
s1503189 0:c727f4699e80 114
s1503189 0:c727f4699e80 115
s1503189 0:c727f4699e80 116 double Force_prev;
s1503189 0:c727f4699e80 117 double Input_prev3;
s1503189 0:c727f4699e80 118
s1503189 1:57926254e65b 119 double Inputberekening3(double B)
s1503189 1:57926254e65b 120 {
s1503189 0:c727f4699e80 121 if (Force_nieuw>=2.5) {
s1503189 0:c727f4699e80 122 if (A == 1) {
s1503189 0:c727f4699e80 123 Input3 = Input_prev3-0.01;
s1503189 0:c727f4699e80 124 }
s1503189 0:c727f4699e80 125 if (A == 0 && Force_prev>2.5) {
s1503189 0:c727f4699e80 126 A = 0;
s1503189 0:c727f4699e80 127 }
s1503189 0:c727f4699e80 128 if (A <= 0 && Force_prev<=2.5) {
s1503189 0:c727f4699e80 129 A++;
s1503189 0:c727f4699e80 130 Input3 = Input_prev3+0.01;
s1503189 0:c727f4699e80 131 }
s1503189 1:57926254e65b 132 } else if (Force_nieuw<=-2.5) {
s1503189 1:57926254e65b 133 if (A == -1) {
s1503189 1:57926254e65b 134 Input3 = Input_prev3+0.01;
s1503189 1:57926254e65b 135 }
s1503189 1:57926254e65b 136 if (A == 0 && Force_prev<-2.5) {
s1503189 1:57926254e65b 137 A = 0;
s1503189 1:57926254e65b 138 }
s1503189 0:c727f4699e80 139 if (A >= 0 && Force_prev>=-2.5) {
s1503189 0:c727f4699e80 140 A--;
s1503189 0:c727f4699e80 141 Input3 = Input_prev3-0.01;
s1503189 0:c727f4699e80 142 }
s1503189 1:57926254e65b 143 } else {
s1503189 0:c727f4699e80 144 Input3 = Input_prev3 - A*0.01;
s1503189 0:c727f4699e80 145 }
s1503189 0:c727f4699e80 146 Force_prev= Force_nieuw;
s1503189 0:c727f4699e80 147 if (Input3 >= 9) {
s1503189 0:c727f4699e80 148 Input3 =9;
s1503189 0:c727f4699e80 149 }
s1503189 0:c727f4699e80 150 if (Input3 <= 3.5) {
s1503189 0:c727f4699e80 151 Input3 =3.5;
s1503189 0:c727f4699e80 152 }
s1503189 0:c727f4699e80 153 Input_prev3 = Input3;
s1503189 0:c727f4699e80 154 return Input3;
s1503189 0:c727f4699e80 155
s1503189 0:c727f4699e80 156 }
s1503189 0:c727f4699e80 157
s1503189 0:c727f4699e80 158 double Errorberekening(double Ref,double Input)
s1503189 0:c727f4699e80 159 {
s1503189 0:c727f4699e80 160 Error = Input-Ref; // Het Error-signaal wordt ook gebruikt voor de PWMOut, dus mag deze niet hoger worden dan 1, 1 is immers al full speed voor de motor.
s1503189 1:57926254e65b 161
s1503189 0:c727f4699e80 162 return Error;
s1503189 0:c727f4699e80 163 }
s1503189 0:c727f4699e80 164 double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev)
s1503189 0:c727f4699e80 165 {
s1503189 0:c727f4699e80 166 Error_der = (Error-Error_prev)/Ts;
s1503189 0:c727f4699e80 167 Error_prev = Error;
s1503189 0:c727f4699e80 168 Error_int = Error_int + Ts*Error;
s1503189 0:c727f4699e80 169
s1503189 0:c727f4699e80 170 return KP*Error+KI*Ts*Error_int+KD*Ts*Error_der;
s1503189 0:c727f4699e80 171 }
s1503189 0:c727f4699e80 172
s1503189 0:c727f4699e80 173 void Afgeleide_Force()
s1503189 0:c727f4699e80 174 {
s1503189 0:c727f4699e80 175 Y_filtder = (Y_filt-Y_filtprev)/0.01;
s1503189 0:c727f4699e80 176 Y_filtprev = Y_filt;
s1503189 0:c727f4699e80 177 }
s1503189 0:c727f4699e80 178
s1503189 0:c727f4699e80 179 void Afgeleide_Plaats()
s1503189 0:c727f4699e80 180 {
s1503189 0:c727f4699e80 181 LoopTimerFlag2 = 0;
s1503189 1:57926254e65b 182 Plaats_der = Ref_der/0.01;
s1503189 0:c727f4699e80 183 Plaats_der1 = Filter6.step(Plaats_der);
s1503189 0:c727f4699e80 184 Plaats_der = (Plaats_der1-Ref_der_prev)/0.1;
s1503189 0:c727f4699e80 185 if (Plaats_der>=0) {
s1503189 0:c727f4699e80 186 Plaats_der = Plaats_der;
s1503189 0:c727f4699e80 187 } else if (Plaats_der<0) {
s1503189 0:c727f4699e80 188 Plaats_der = 0.5*Plaats_der;
s1503189 0:c727f4699e80 189 }
s1503189 0:c727f4699e80 190 Ref_der_prev= Plaats_der1;
s1503189 0:c727f4699e80 191 }
s1503189 0:c727f4699e80 192
s1503189 0:c727f4699e80 193 void Motor_controller()
s1503189 0:c727f4699e80 194 {
s1503189 0:c727f4699e80 195 if(Output>=0) {
s1503189 0:c727f4699e80 196 motor1direction.write(1);
s1503189 0:c727f4699e80 197 motor1speed = 0.2+fabs(Output);
s1503189 0:c727f4699e80 198 } else if (Output<0) {
s1503189 0:c727f4699e80 199 motor1direction.write(0);
s1503189 5:d1c6f7fef412 200 motor1speed = fabs(Output);
s1503189 0:c727f4699e80 201 }
s1503189 0:c727f4699e80 202 }
s1503189 0:c727f4699e80 203
s1503189 0:c727f4699e80 204 int main()
s1503189 0:c727f4699e80 205 {
s1503189 0:c727f4699e80 206 Roodisplus.write(1);
s1503189 0:c727f4699e80 207 Versterker.write(1);
s1503189 0:c727f4699e80 208 // Attach the data read and send function at 100 Hz
s1503189 0:c727f4699e80 209 scopeTimer.attach_us(&tickerfunctie, 1/0.0003);// 1e4);
s1503189 0:c727f4699e80 210 ForceTimer.attach_us(&tickerfunctie2, 1e4);
s1503189 0:c727f4699e80 211 while(1) {
s1503189 0:c727f4699e80 212 while(LoopTimerFlag !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 0:c727f4699e80 213 LoopTimerFlag = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
s1503189 0:c727f4699e80 214 Led = 1;
s1503189 0:c727f4699e80 215 Y_filt = 3*Filter1.step(a_in3.read());
s1503189 0:c727f4699e80 216 Y_filt2 = Filter2.step(Y_filt);
s1503189 1:57926254e65b 217 Input = Inputberekening3(Force_nieuw);
s1503189 0:c727f4699e80 218 time_ += time_increment;
s1503189 0:c727f4699e80 219 Ref2 = Referentieschaling(Referentie.read()/2,Input); // De referentiewaarde is via deze functie (gevonden met metingen en polyfit) verbonden aan de afstand in centimeters voor waarden tussen 0 en 0.5.
s1503189 0:c727f4699e80 220 Error = Errorberekening(Ref2, Input);
s1503189 0:c727f4699e80 221 Ref_der = Referentie2-Ref_prev;
s1503189 0:c727f4699e80 222 Ref_prev = Referentie2;
s1503189 6:35d5bab53f44 223 Output = PID_controller(Error,0.5,0.1,0.05,0.01, Error_prev, Error_int);
s1503189 0:c727f4699e80 224 Motor_controller();
s1503189 0:c727f4699e80 225 if (LoopTimerFlag2==1) {
s1503189 0:c727f4699e80 226 Afgeleide_Force();
s1503189 0:c727f4699e80 227 Afgeleide_Plaats();
s1503189 0:c727f4699e80 228 Force_nieuw = Y_filtder-Boardpotmeter2.read()*Plaats_der;
s1503189 0:c727f4699e80 229 }
s1503189 0:c727f4699e80 230 scopeSend();
s1503189 0:c727f4699e80 231 }
s1503189 0:c727f4699e80 232
s1503189 0:c727f4699e80 233
s1503189 0:c727f4699e80 234 }