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@3:5b5b8ebdc46c, 2016-06-10 (annotated)

Committer:
s1503189
Date:
Fri Jun 10 12:31:08 2016 +0000
Revision:
3:5b5b8ebdc46c
Parent:
2:f5bb7fcfc7a2 
Kapot, filters gelijk getrokken, waardes offset gegeven, andere signalen van elkaar afgetrokken om het te verklaren te maken

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 3:5b5b8ebdc46c 42 double Plaats_der2;
s1503189 0:c727f4699e80 43
s1503189 0:c727f4699e80 44 //LowPass filter 2 Hz
s1503189 0:c727f4699e80 45 const double a1_LP = -1.940778135263835, a2_LP = 0.942482022027066;
s1503189 0:c727f4699e80 46 const double b0_LP = 0.000425971690807714, b1_LP = 0.000851943381615428, b2_LP = 0.000425971690807714;
s1503189 0:c727f4699e80 47 const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125;
s1503189 0:c727f4699e80 48 const double b0_HP = 0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;
s1503189 1:57926254e65b 49 const double a1_LP5 = -1.822694925196308, a2_LP5 = 0.837181651256023 ;
s1503189 1:57926254e65b 50 const double b0_LP5 = 0.003621681514929 , b1_LP5 = 0.007243363029857, b2_LP5 = 0.003621681514929;
s1503189 0:c727f4699e80 51 const double a1_LP2 = -1.561018075800718, a2_LP2 = 0.641351538057563;
s1503189 0:c727f4699e80 52 const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211;
s1503189 1:57926254e65b 53
s1503189 0:c727f4699e80 54
s1503189 3:5b5b8ebdc46c 55 biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); //(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); // Lowpassfilter voor signaal van krachtsensor, 300 Hz, fk = 2
s1503189 3:5b5b8ebdc46c 56 biquadFilter Filter2(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); //(a1_HP, a2_HP, b0_HP, b1_HP, b2_HP); // Highpassfilter voor signaal van krachtsensor, 300 Hz, fk = 10
s1503189 1:57926254e65b 57 biquadFilter Filter3(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2); // Lowpassfilter voor signaal positie-potmeter, 300 Hz
s1503189 3:5b5b8ebdc46c 58 biquadFilter Filter4(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP); // Lowpassfilter voor geschaalde signaal positie-potmeter, 300 Hz
s1503189 1:57926254e65b 59 biquadFilter Filter6(a1_LP5, a2_LP5, b0_LP5, b1_LP5, b2_LP5); // Laagdoorlaatfilter 100 Hz, fk is 2 Hz
s1503189 1:57926254e65b 60
s1503189 3:5b5b8ebdc46c 61 /* Aanpasmogelijkheden die succes kunnen geven:
s1503189 3:5b5b8ebdc46c 62 -Positiesignaal ook door highpassfilter gooien, filters voor krachtsensor en positie gelijkstellen.
s1503189 3:5b5b8ebdc46c 63 -Lowpassfilter van geschaalde signaal weghalen.
s1503189 3:5b5b8ebdc46c 64 - Signalen bekijken van referentie, ref_der en de tweede afgeleide. Wellicht ook derde afgeleide bekijken.
s1503189 3:5b5b8ebdc46c 65 -
s1503189 3:5b5b8ebdc46c 66 */
s1503189 0:c727f4699e80 67
s1503189 0:c727f4699e80 68 double Y_filt;
s1503189 0:c727f4699e80 69 double Y_filt2;
s1503189 0:c727f4699e80 70 double Y_filtprev;
s1503189 0:c727f4699e80 71 double Y_filtder;
s1503189 0:c727f4699e80 72 int A;
s1503189 0:c727f4699e80 73 float time_;
s1503189 0:c727f4699e80 74 const float time_increment = 0.005;
s1503189 0:c727f4699e80 75
s1503189 0:c727f4699e80 76 // The data read and send function
s1503189 0:c727f4699e80 77 void scopeSend()
s1503189 0:c727f4699e80 78 {
s1503189 3:5b5b8ebdc46c 79 scope.set(0,Force_nieuw); //Y_filt);
s1503189 3:5b5b8ebdc46c 80 scope.set(1,Y_filt2);
s1503189 3:5b5b8ebdc46c 81 scope.set(2,Plaats_der1);
s1503189 3:5b5b8ebdc46c 82 scope.set(3,Input);//Referentie2);//
s1503189 3:5b5b8ebdc46c 83 scope.set(4,A);//Y_filtder);//
s1503189 0:c727f4699e80 84 scope.send();
s1503189 0:c727f4699e80 85
s1503189 0:c727f4699e80 86 }
s1503189 0:c727f4699e80 87
s1503189 0:c727f4699e80 88 void tickerfunctie() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
s1503189 0:c727f4699e80 89 {
s1503189 0:c727f4699e80 90 LoopTimerFlag = 1;
s1503189 0:c727f4699e80 91 }
s1503189 0:c727f4699e80 92
s1503189 0:c727f4699e80 93 void tickerfunctie2() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
s1503189 0:c727f4699e80 94 {
s1503189 0:c727f4699e80 95 LoopTimerFlag2 = 1;
s1503189 0:c727f4699e80 96 }
s1503189 0:c727f4699e80 97
s1503189 0:c727f4699e80 98
s1503189 0:c727f4699e80 99 double Referentieschaling(double A, double B)
s1503189 0:c727f4699e80 100 {
s1503189 0:c727f4699e80 101 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 102 Referentie2 = Filter4.step(Y);
s1503189 0:c727f4699e80 103 if (Referentie2<=2.5 or Referentie2>=9.2) {
s1503189 0:c727f4699e80 104 Led = 0;
s1503189 0:c727f4699e80 105 Referentie2 = B;
s1503189 0:c727f4699e80 106 }
s1503189 0:c727f4699e80 107 return Referentie2;
s1503189 0:c727f4699e80 108 }
s1503189 0:c727f4699e80 109
s1503189 0:c727f4699e80 110 double Inputberekening(double B)
s1503189 0:c727f4699e80 111 {
s1503189 0:c727f4699e80 112 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 113 Input = Filter3.step(Inp);
s1503189 0:c727f4699e80 114 if (Input>=9) { // De Input moet binnnen een aantal grenzen blijven. Groter dan 7 is mechanisch niet mogelijk.
s1503189 0:c727f4699e80 115 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 116 } else if (Input<=3.0) {
s1503189 0:c727f4699e80 117 Input = 3.0;
s1503189 0:c727f4699e80 118 }
s1503189 0:c727f4699e80 119 return Input;
s1503189 0:c727f4699e80 120 }
s1503189 0:c727f4699e80 121
s1503189 0:c727f4699e80 122
s1503189 0:c727f4699e80 123 double Force_prev;
s1503189 0:c727f4699e80 124 double Input_prev3;
s1503189 0:c727f4699e80 125
s1503189 1:57926254e65b 126 double Inputberekening3(double B)
s1503189 1:57926254e65b 127 {
s1503189 0:c727f4699e80 128 if (Force_nieuw>=2.5) {
s1503189 0:c727f4699e80 129 if (A == 1) {
s1503189 0:c727f4699e80 130 Input3 = Input_prev3-0.01;
s1503189 0:c727f4699e80 131 }
s1503189 0:c727f4699e80 132 if (A == 0 && Force_prev>2.5) {
s1503189 0:c727f4699e80 133 A = 0;
s1503189 0:c727f4699e80 134 }
s1503189 0:c727f4699e80 135 if (A <= 0 && Force_prev<=2.5) {
s1503189 0:c727f4699e80 136 A++;
s1503189 0:c727f4699e80 137 Input3 = Input_prev3+0.01;
s1503189 0:c727f4699e80 138 }
s1503189 1:57926254e65b 139 } else if (Force_nieuw<=-2.5) {
s1503189 1:57926254e65b 140 if (A == -1) {
s1503189 1:57926254e65b 141 Input3 = Input_prev3+0.01;
s1503189 1:57926254e65b 142 }
s1503189 1:57926254e65b 143 if (A == 0 && Force_prev<-2.5) {
s1503189 1:57926254e65b 144 A = 0;
s1503189 1:57926254e65b 145 }
s1503189 0:c727f4699e80 146 if (A >= 0 && Force_prev>=-2.5) {
s1503189 0:c727f4699e80 147 A--;
s1503189 0:c727f4699e80 148 Input3 = Input_prev3-0.01;
s1503189 0:c727f4699e80 149 }
s1503189 1:57926254e65b 150 } else {
s1503189 0:c727f4699e80 151 Input3 = Input_prev3 - A*0.01;
s1503189 0:c727f4699e80 152 }
s1503189 0:c727f4699e80 153 Force_prev= Force_nieuw;
s1503189 0:c727f4699e80 154 if (Input3 >= 9) {
s1503189 0:c727f4699e80 155 Input3 =9;
s1503189 0:c727f4699e80 156 }
s1503189 0:c727f4699e80 157 if (Input3 <= 3.5) {
s1503189 0:c727f4699e80 158 Input3 =3.5;
s1503189 0:c727f4699e80 159 }
s1503189 0:c727f4699e80 160 Input_prev3 = Input3;
s1503189 0:c727f4699e80 161 return Input3;
s1503189 0:c727f4699e80 162
s1503189 0:c727f4699e80 163 }
s1503189 0:c727f4699e80 164
s1503189 0:c727f4699e80 165 double Errorberekening(double Ref,double Input)
s1503189 0:c727f4699e80 166 {
s1503189 0:c727f4699e80 167 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 168
s1503189 0:c727f4699e80 169 return Error;
s1503189 0:c727f4699e80 170 }
s1503189 0:c727f4699e80 171 double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev)
s1503189 0:c727f4699e80 172 {
s1503189 0:c727f4699e80 173 Error_der = (Error-Error_prev)/Ts;
s1503189 0:c727f4699e80 174 Error_prev = Error;
s1503189 0:c727f4699e80 175 Error_int = Error_int + Ts*Error;
s1503189 0:c727f4699e80 176
s1503189 0:c727f4699e80 177 return KP*Error+KI*Ts*Error_int+KD*Ts*Error_der;
s1503189 0:c727f4699e80 178 }
s1503189 0:c727f4699e80 179
s1503189 0:c727f4699e80 180 void Afgeleide_Force()
s1503189 0:c727f4699e80 181 {
s1503189 3:5b5b8ebdc46c 182 Y_filtder = (Force_nieuw-Y_filtprev)/0.01;
s1503189 3:5b5b8ebdc46c 183 Y_filtprev = Force_nieuw;
s1503189 0:c727f4699e80 184 }
s1503189 0:c727f4699e80 185
s1503189 0:c727f4699e80 186 void Afgeleide_Plaats()
s1503189 0:c727f4699e80 187 {
s1503189 0:c727f4699e80 188 LoopTimerFlag2 = 0;
s1503189 3:5b5b8ebdc46c 189 Plaats_der = Ref_der/0.1;
s1503189 3:5b5b8ebdc46c 190 Plaats_der1 = (Filter6.step(Plaats_der)/(Boardpotmeter2.read()*5))-1;
s1503189 3:5b5b8ebdc46c 191 Plaats_der2 = (Plaats_der1-Ref_der_prev);
s1503189 3:5b5b8ebdc46c 192 if (Plaats_der1>=-1) {
s1503189 3:5b5b8ebdc46c 193 Plaats_der1 = Plaats_der1;
s1503189 3:5b5b8ebdc46c 194 } else if (Plaats_der1<-1) {
s1503189 3:5b5b8ebdc46c 195 Plaats_der1 = (Filter6.step(Plaats_der)/(Boardpotmeter2.read()*5))*0.5-1;
s1503189 0:c727f4699e80 196 }
s1503189 0:c727f4699e80 197 Ref_der_prev= Plaats_der1;
s1503189 0:c727f4699e80 198 }
s1503189 0:c727f4699e80 199
s1503189 0:c727f4699e80 200 void Motor_controller()
s1503189 0:c727f4699e80 201 {
s1503189 0:c727f4699e80 202 if(Output>=0) {
s1503189 0:c727f4699e80 203 motor1direction.write(1);
s1503189 0:c727f4699e80 204 motor1speed = 0.2+fabs(Output);
s1503189 0:c727f4699e80 205 } else if (Output<0) {
s1503189 0:c727f4699e80 206 motor1direction.write(0);
s1503189 0:c727f4699e80 207 motor1speed = 0.05*fabs(Output);
s1503189 0:c727f4699e80 208 }
s1503189 0:c727f4699e80 209 }
s1503189 0:c727f4699e80 210
s1503189 0:c727f4699e80 211 int main()
s1503189 0:c727f4699e80 212 {
s1503189 0:c727f4699e80 213 Roodisplus.write(1);
s1503189 0:c727f4699e80 214 Versterker.write(1);
s1503189 0:c727f4699e80 215 // Attach the data read and send function at 100 Hz
s1503189 0:c727f4699e80 216 scopeTimer.attach_us(&tickerfunctie, 1/0.0003);// 1e4);
s1503189 0:c727f4699e80 217 ForceTimer.attach_us(&tickerfunctie2, 1e4);
s1503189 0:c727f4699e80 218 while(1) {
s1503189 0:c727f4699e80 219 while(LoopTimerFlag !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 0:c727f4699e80 220 LoopTimerFlag = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
s1503189 0:c727f4699e80 221 Led = 1;
s1503189 3:5b5b8ebdc46c 222 Y_filt = 5*Filter1.step(a_in3.read())+1;
s1503189 0:c727f4699e80 223 Y_filt2 = Filter2.step(Y_filt);
s1503189 1:57926254e65b 224 Input = Inputberekening3(Force_nieuw);
s1503189 0:c727f4699e80 225 time_ += time_increment;
s1503189 0:c727f4699e80 226 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 227 Error = Errorberekening(Ref2, Input);
s1503189 0:c727f4699e80 228 Ref_der = Referentie2-Ref_prev;
s1503189 0:c727f4699e80 229 Ref_prev = Referentie2;
s1503189 0:c727f4699e80 230 Output = PID_controller(Error,3,2,1,0.01, Error_prev, Error_int);
s1503189 0:c727f4699e80 231 Motor_controller();
s1503189 0:c727f4699e80 232 if (LoopTimerFlag2==1) {
s1503189 0:c727f4699e80 233 Afgeleide_Force();
s1503189 0:c727f4699e80 234 Afgeleide_Plaats();
s1503189 3:5b5b8ebdc46c 235 Force_nieuw = Y_filtder-Plaats_der2;
s1503189 0:c727f4699e80 236 }
s1503189 0:c727f4699e80 237 scopeSend();
s1503189 0:c727f4699e80 238 }
s1503189 0:c727f4699e80 239
s1503189 0:c727f4699e80 240
s1503189 0:c727f4699e80 241 }