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@0:c727f4699e80, 2016-06-08 (annotated)

Committer:
s1503189
Date:
Wed Jun 08 11:43:46 2016 +0000
Revision:
0:c727f4699e80
Child:
1:57926254e65b
Werkende aansturing

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 //Serial pc(USBTX, USBRX);
s1503189 0:c727f4699e80 13
s1503189 0:c727f4699e80 14 // Read the analog input
s1503189 0:c727f4699e80 15 //AnalogIn a_in(A0); // Potmeter
s1503189 0:c727f4699e80 16 AnalogIn a_in2(A3); //OUT-
s1503189 0:c727f4699e80 17 AnalogIn a_in3(A4); //OUT+
s1503189 0:c727f4699e80 18
s1503189 0:c727f4699e80 19 Ticker Loopticker; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
s1503189 0:c727f4699e80 20 AnalogIn Referentie(A0); // De schuifpotmeter naast de geleiding van de draad is de referentie.
s1503189 0:c727f4699e80 21 AnalogIn Boardpotmeter(A1); // POT1 op het board wordt gebruikt als input van de controlloop.
s1503189 0:c727f4699e80 22 AnalogIn Boardpotmeter2(A2);
s1503189 0:c727f4699e80 23 DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
s1503189 0:c727f4699e80 24 FastPWM motor1speed(D6);
s1503189 0:c727f4699e80 25 DigitalOut Led(LED_RED);
s1503189 0:c727f4699e80 26 DigitalOut Roodisplus(D5);
s1503189 0:c727f4699e80 27 DigitalOut Versterker(D4);
s1503189 0:c727f4699e80 28 float potmeter;
s1503189 0:c727f4699e80 29 float Error;
s1503189 0:c727f4699e80 30 float Referentie2;//=1.0;
s1503189 0:c727f4699e80 31 float Output;
s1503189 0:c727f4699e80 32 float Input;
s1503189 0:c727f4699e80 33 double Error_prev = 0;
s1503189 0:c727f4699e80 34 double Error_int = 0;
s1503189 0:c727f4699e80 35 double Error_der;
s1503189 0:c727f4699e80 36 double Ref_der = 0;
s1503189 0:c727f4699e80 37 double Ref_prev = 0;
s1503189 0:c727f4699e80 38 double Ref_prev2 = 0;
s1503189 0:c727f4699e80 39 double Ref2;
s1503189 0:c727f4699e80 40 double Input3;
s1503189 0:c727f4699e80 41 double Input_der=0;
s1503189 0:c727f4699e80 42 double Input_prev=0;
s1503189 0:c727f4699e80 43 double Input_prev2=4;
s1503189 0:c727f4699e80 44 double Plaats_der;
s1503189 0:c727f4699e80 45 double Force_nieuw=0;
s1503189 0:c727f4699e80 46 double Plaats_acc;
s1503189 0:c727f4699e80 47 double Ref_der_prev;
s1503189 0:c727f4699e80 48 double Plaats_der1;
s1503189 0:c727f4699e80 49
s1503189 0:c727f4699e80 50 //LowPass filter 2 Hz
s1503189 0:c727f4699e80 51 //double low1_v1 = 0, low1_v2 = 0;
s1503189 0:c727f4699e80 52 //double low2_v1 = 0, low2_v2 = 0;
s1503189 0:c727f4699e80 53 const double a1_LP = -1.940778135263835, a2_LP = 0.942482022027066;
s1503189 0:c727f4699e80 54 const double b0_LP = 0.000425971690807714, b1_LP = 0.000851943381615428, b2_LP = 0.000425971690807714;
s1503189 0:c727f4699e80 55 const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125;
s1503189 0:c727f4699e80 56 const double b0_HP = 0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;
s1503189 0:c727f4699e80 57 const double a1_LP5 = -1.822694925196308, a2_LP5 = 0.837181651256023 ; //-1.911197067426073, a2_LP5 = 0.914975834801434;
s1503189 0:c727f4699e80 58 const double b0_LP5 = 0.003621681514929 , b1_LP5 = 0.007243363029857, b2_LP5 = 0.003621681514929; // 0.000944691843840, b1_LP5 = 0.001889383687680 , b2_LP5 = 0.000944691843840;
s1503189 0:c727f4699e80 59
s1503189 0:c727f4699e80 60 const double a1_LP3 = -1.561018075800718, a2_LP3 = 0.641351538057563;
s1503189 0:c727f4699e80 61 const double b0_LP3 = 0.020083365564211, b1_LP3 = 0.040166731128423, b2_LP3 = 0.020083365564211;
s1503189 0:c727f4699e80 62 const double a1_LP2 = -1.561018075800718, a2_LP2 = 0.641351538057563;
s1503189 0:c727f4699e80 63 const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211;
s1503189 0:c727f4699e80 64 const double a1_LP4 = -1.561018075800718, a2_LP4 = 0.641351538057563;
s1503189 0:c727f4699e80 65 const double b0_LP4 = 0.020083365564211, b1_LP4 = 0.040166731128423, b2_LP4 = 0.020083365564211;
s1503189 0:c727f4699e80 66
s1503189 0:c727f4699e80 67 /*
s1503189 0:c727f4699e80 68 const double a1_LP = -1.911197067426073, a2_LP = 0.914975834801434;
s1503189 0:c727f4699e80 69 const double b0_LP = 0.067455273889072, b1_LP = 0.134910547778144, b2_LP = 0.067455273889072;
s1503189 0:c727f4699e80 70 const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125;
s1503189 0:c727f4699e80 71 const double b0_HP = 0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;
s1503189 0:c727f4699e80 72 */
s1503189 0:c727f4699e80 73 biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);
s1503189 0:c727f4699e80 74 biquadFilter Filter2(a1_HP, a2_HP, b0_HP, b1_HP, b2_HP);
s1503189 0:c727f4699e80 75 biquadFilter Filter3(a1_LP3, a2_LP3, b0_LP3, b1_LP3, b2_LP3);
s1503189 0:c727f4699e80 76 biquadFilter Filter4(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);
s1503189 0:c727f4699e80 77 biquadFilter Filter5(a1_LP4, a2_LP4, b0_LP4, b1_LP4, b2_LP4);
s1503189 0:c727f4699e80 78 biquadFilter Filter6(a1_LP5, a2_LP5, b0_LP5, b1_LP5, b2_LP5); // fk is 2 Hz
s1503189 0:c727f4699e80 79 biquadFilter Filter7(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);
s1503189 0:c727f4699e80 80
s1503189 0:c727f4699e80 81 double Y_filt;
s1503189 0:c727f4699e80 82 double Y_filt2;
s1503189 0:c727f4699e80 83 double Y_filtprev;
s1503189 0:c727f4699e80 84 double Y_filtprev2;
s1503189 0:c727f4699e80 85 double Y_filtprev3;
s1503189 0:c727f4699e80 86 double Y_filtprev4;
s1503189 0:c727f4699e80 87 double Y_filtprev5;
s1503189 0:c727f4699e80 88 double Y_filtder;
s1503189 0:c727f4699e80 89 int A;
s1503189 0:c727f4699e80 90 float time_;
s1503189 0:c727f4699e80 91 const float time_increment = 0.005;
s1503189 0:c727f4699e80 92
s1503189 0:c727f4699e80 93 // The data read and send function
s1503189 0:c727f4699e80 94 void scopeSend()
s1503189 0:c727f4699e80 95 {
s1503189 0:c727f4699e80 96 scope.set(0,Force_nieuw);
s1503189 0:c727f4699e80 97 //scope.set(1,Input);
s1503189 0:c727f4699e80 98 scope.set(3,Input);//Boardpotmeter2.read());
s1503189 0:c727f4699e80 99 scope.set(1,Y_filtder);
s1503189 0:c727f4699e80 100 scope.set(2,Plaats_der);
s1503189 0:c727f4699e80 101 scope.set(4,A);
s1503189 0:c727f4699e80 102 scope.send();
s1503189 0:c727f4699e80 103
s1503189 0:c727f4699e80 104 }
s1503189 0:c727f4699e80 105
s1503189 0:c727f4699e80 106 void tickerfunctie() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
s1503189 0:c727f4699e80 107 {
s1503189 0:c727f4699e80 108 LoopTimerFlag = 1;
s1503189 0:c727f4699e80 109 }
s1503189 0:c727f4699e80 110
s1503189 0:c727f4699e80 111 void tickerfunctie2() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
s1503189 0:c727f4699e80 112 {
s1503189 0:c727f4699e80 113 LoopTimerFlag2 = 1;
s1503189 0:c727f4699e80 114 }
s1503189 0:c727f4699e80 115
s1503189 0:c727f4699e80 116
s1503189 0:c727f4699e80 117 double Referentieschaling(double A, double B)
s1503189 0:c727f4699e80 118 {
s1503189 0:c727f4699e80 119 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 120 Referentie2 = Filter4.step(Y);
s1503189 0:c727f4699e80 121 if (Referentie2<=2.5 or Referentie2>=9.2) {
s1503189 0:c727f4699e80 122 Led = 0;
s1503189 0:c727f4699e80 123 Referentie2 = B;
s1503189 0:c727f4699e80 124 }
s1503189 0:c727f4699e80 125 return Referentie2;
s1503189 0:c727f4699e80 126 }
s1503189 0:c727f4699e80 127
s1503189 0:c727f4699e80 128 double Inputberekening(double B)
s1503189 0:c727f4699e80 129 {
s1503189 0:c727f4699e80 130 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 131 Input = Filter3.step(Inp);
s1503189 0:c727f4699e80 132 //Input = 6+(2.5*sin(time_));
s1503189 0:c727f4699e80 133 if (Input>=9) { // De Input moet binnnen een aantal grenzen blijven. Groter dan 7 is mechanisch niet mogelijk.
s1503189 0:c727f4699e80 134 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 135 } else if (Input<=3.0) {
s1503189 0:c727f4699e80 136 Input = 3.0;
s1503189 0:c727f4699e80 137 }
s1503189 0:c727f4699e80 138 return Input;
s1503189 0:c727f4699e80 139 }
s1503189 0:c727f4699e80 140
s1503189 0:c727f4699e80 141 /*double Input2(double B)
s1503189 0:c727f4699e80 142 {
s1503189 0:c727f4699e80 143 // if (Y_filtder
s1503189 0:c727f4699e80 144 if (Y_filtder>=1) {
s1503189 0:c727f4699e80 145 Input3 = Input_prev2+0.01;
s1503189 0:c727f4699e80 146 wait(0);
s1503189 0:c727f4699e80 147 }
s1503189 0:c727f4699e80 148 if (Y_filtder<= -1) {
s1503189 0:c727f4699e80 149 Input3 = Input_prev2-0.01;
s1503189 0:c727f4699e80 150 wait(0);
s1503189 0:c727f4699e80 151 }
s1503189 0:c727f4699e80 152 if (Input3 >= 9) {
s1503189 0:c727f4699e80 153 Input3 =9;
s1503189 0:c727f4699e80 154 }
s1503189 0:c727f4699e80 155 if (Input3 <= 2.5) {
s1503189 0:c727f4699e80 156 Input3 =2.5;
s1503189 0:c727f4699e80 157 }
s1503189 0:c727f4699e80 158
s1503189 0:c727f4699e80 159 Input_prev2 = Input3;
s1503189 0:c727f4699e80 160 return Input3;
s1503189 0:c727f4699e80 161 }*/
s1503189 0:c727f4699e80 162
s1503189 0:c727f4699e80 163
s1503189 0:c727f4699e80 164 double Force_prev;
s1503189 0:c727f4699e80 165 double Input_prev3;
s1503189 0:c727f4699e80 166
s1503189 0:c727f4699e80 167 double Inputberekening3(double B){
s1503189 0:c727f4699e80 168 if (Force_nieuw>=2.5) {
s1503189 0:c727f4699e80 169 if (A == 1) {
s1503189 0:c727f4699e80 170 Input3 = Input_prev3-0.01;
s1503189 0:c727f4699e80 171 }
s1503189 0:c727f4699e80 172 if (A == 0 && Force_prev>2.5) {
s1503189 0:c727f4699e80 173 A = 0;
s1503189 0:c727f4699e80 174 //pc.printf("A = 0 en Forceprev>1.5 \n");
s1503189 0:c727f4699e80 175 }
s1503189 0:c727f4699e80 176 if (A <= 0 && Force_prev<=2.5) {
s1503189 0:c727f4699e80 177 A++;
s1503189 0:c727f4699e80 178 Input3 = Input_prev3+0.01;
s1503189 0:c727f4699e80 179 // pc.printf("A = 0 en Forceprev<1.5 \n");
s1503189 0:c727f4699e80 180 }
s1503189 0:c727f4699e80 181 }
s1503189 0:c727f4699e80 182 else if (Force_nieuw<=-2.5) {
s1503189 0:c727f4699e80 183 if (A == -1)
s1503189 0:c727f4699e80 184 { Input3 = Input_prev3+0.01;
s1503189 0:c727f4699e80 185 }
s1503189 0:c727f4699e80 186 if (A == 0 && Force_prev<-2.5) {
s1503189 0:c727f4699e80 187 A = 0;
s1503189 0:c727f4699e80 188 // pc.printf("A = 0 en Forceprev<1.5 Force<-1.5 \n");
s1503189 0:c727f4699e80 189 }
s1503189 0:c727f4699e80 190 if (A >= 0 && Force_prev>=-2.5) {
s1503189 0:c727f4699e80 191 A--;
s1503189 0:c727f4699e80 192 Input3 = Input_prev3-0.01;
s1503189 0:c727f4699e80 193 // pc.printf("A = 0 en Forceprev<1.5 Force<-1.5 \n");
s1503189 0:c727f4699e80 194 }
s1503189 0:c727f4699e80 195 }
s1503189 0:c727f4699e80 196 else {
s1503189 0:c727f4699e80 197 Input3 = Input_prev3 - A*0.01;
s1503189 0:c727f4699e80 198 // pc.printf("Else A = %i \n", A);
s1503189 0:c727f4699e80 199 }
s1503189 0:c727f4699e80 200 Force_prev= Force_nieuw;
s1503189 0:c727f4699e80 201 if (Input3 >= 9) {
s1503189 0:c727f4699e80 202 Input3 =9;
s1503189 0:c727f4699e80 203 }
s1503189 0:c727f4699e80 204 if (Input3 <= 3.5) {
s1503189 0:c727f4699e80 205 Input3 =3.5;
s1503189 0:c727f4699e80 206 }
s1503189 0:c727f4699e80 207 Input_prev3 = Input3;
s1503189 0:c727f4699e80 208 return Input3;
s1503189 0:c727f4699e80 209
s1503189 0:c727f4699e80 210 }
s1503189 0:c727f4699e80 211
s1503189 0:c727f4699e80 212 double Errorberekening(double Ref,double Input)
s1503189 0:c727f4699e80 213 {
s1503189 0:c727f4699e80 214 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 0:c727f4699e80 215 /* if (Error>=1) {
s1503189 0:c727f4699e80 216 Error=1;
s1503189 0:c727f4699e80 217 } else if (Error<=-1) {
s1503189 0:c727f4699e80 218 Error = -1;
s1503189 0:c727f4699e80 219 }
s1503189 0:c727f4699e80 220 else if (fabs(Error)<0.01)
s1503189 0:c727f4699e80 221 { Error = 0; }*/
s1503189 0:c727f4699e80 222 return Error;
s1503189 0:c727f4699e80 223 }
s1503189 0:c727f4699e80 224 double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev)
s1503189 0:c727f4699e80 225 {
s1503189 0:c727f4699e80 226 Error_der = (Error-Error_prev)/Ts;
s1503189 0:c727f4699e80 227 Error_prev = Error;
s1503189 0:c727f4699e80 228 Error_int = Error_int + Ts*Error;
s1503189 0:c727f4699e80 229
s1503189 0:c727f4699e80 230 return KP*Error+KI*Ts*Error_int+KD*Ts*Error_der;
s1503189 0:c727f4699e80 231 }
s1503189 0:c727f4699e80 232
s1503189 0:c727f4699e80 233 void Afgeleide_Force()
s1503189 0:c727f4699e80 234 {
s1503189 0:c727f4699e80 235 //while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 0:c727f4699e80 236 //LoopTimerFlag2 = 0;
s1503189 0:c727f4699e80 237 Y_filtder = (Y_filt-Y_filtprev)/0.01;
s1503189 0:c727f4699e80 238 //Y_filtder = 3*Filter7.step(Y_filtder);
s1503189 0:c727f4699e80 239 Y_filtprev = Y_filt;
s1503189 0:c727f4699e80 240 }
s1503189 0:c727f4699e80 241
s1503189 0:c727f4699e80 242 void Afgeleide_Plaats()
s1503189 0:c727f4699e80 243 {
s1503189 0:c727f4699e80 244 //while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 0:c727f4699e80 245 LoopTimerFlag2 = 0;
s1503189 0:c727f4699e80 246 Plaats_der = Ref_der/0.01; //(Referentie2-Ref_prev2)/0.1;
s1503189 0:c727f4699e80 247 Plaats_der1 = Filter6.step(Plaats_der);
s1503189 0:c727f4699e80 248 Plaats_der = (Plaats_der1-Ref_der_prev)/0.1;
s1503189 0:c727f4699e80 249 //Plaats_der = Filter5.step(Plaats_acc);
s1503189 0:c727f4699e80 250 if (Plaats_der>=0) {
s1503189 0:c727f4699e80 251 Plaats_der = Plaats_der;
s1503189 0:c727f4699e80 252 } else if (Plaats_der<0) {
s1503189 0:c727f4699e80 253 Plaats_der = 0.5*Plaats_der;
s1503189 0:c727f4699e80 254 }
s1503189 0:c727f4699e80 255 Ref_der_prev= Plaats_der1;
s1503189 0:c727f4699e80 256 }
s1503189 0:c727f4699e80 257
s1503189 0:c727f4699e80 258 void Motor_controller()
s1503189 0:c727f4699e80 259 {
s1503189 0:c727f4699e80 260 if(Output>=0) {
s1503189 0:c727f4699e80 261 motor1direction.write(1);
s1503189 0:c727f4699e80 262 motor1speed = 0.2+fabs(Output);
s1503189 0:c727f4699e80 263 } else if (Output<0) {
s1503189 0:c727f4699e80 264 motor1direction.write(0);
s1503189 0:c727f4699e80 265 motor1speed = 0.05*fabs(Output);
s1503189 0:c727f4699e80 266 }
s1503189 0:c727f4699e80 267 /* if(Input_der < 0)
s1503189 0:c727f4699e80 268 {
s1503189 0:c727f4699e80 269 // motor1direction.write(0);
s1503189 0:c727f4699e80 270 // motor1speed = 0.05*fabs(Output);
s1503189 0:c727f4699e80 271 if(Output>=0)
s1503189 0:c727f4699e80 272 {
s1503189 0:c727f4699e80 273 motor1direction.write(1);
s1503189 0:c727f4699e80 274 motor1speed = 0.3*fabs(Output);
s1503189 0:c727f4699e80 275 }
s1503189 0:c727f4699e80 276 else if (Output<0)
s1503189 0:c727f4699e80 277 {
s1503189 0:c727f4699e80 278 motor1direction.write(0);
s1503189 0:c727f4699e80 279 motor1speed = 0.09*fabs(Output);
s1503189 0:c727f4699e80 280 }
s1503189 0:c727f4699e80 281 }
s1503189 0:c727f4699e80 282 if(Input_der >= 0)
s1503189 0:c727f4699e80 283 {
s1503189 0:c727f4699e80 284 // motor1direction.write(1);
s1503189 0:c727f4699e80 285 //motor1speed = 1;
s1503189 0:c727f4699e80 286 if(Output>=-0.50)
s1503189 0:c727f4699e80 287 {
s1503189 0:c727f4699e80 288 motor1direction.write(1);
s1503189 0:c727f4699e80 289 motor1speed = (fabs(Output)+0.3);
s1503189 0:c727f4699e80 290 }
s1503189 0:c727f4699e80 291 else if (Output<-0.5)
s1503189 0:c727f4699e80 292 {
s1503189 0:c727f4699e80 293 motor1direction.write(0);
s1503189 0:c727f4699e80 294 motor1speed = 0.01*fabs(Output);
s1503189 0:c727f4699e80 295 }
s1503189 0:c727f4699e80 296 }*/
s1503189 0:c727f4699e80 297
s1503189 0:c727f4699e80 298
s1503189 0:c727f4699e80 299 //if (fabs(Output)>=0.01)
s1503189 0:c727f4699e80 300 // motor1speed = fabs(Output);
s1503189 0:c727f4699e80 301 //else if (fabs(Output)<0.01)
s1503189 0:c727f4699e80 302 //{motor1speed= 0;}
s1503189 0:c727f4699e80 303 }
s1503189 0:c727f4699e80 304
s1503189 0:c727f4699e80 305 int main()
s1503189 0:c727f4699e80 306 {
s1503189 0:c727f4699e80 307 Roodisplus.write(1);
s1503189 0:c727f4699e80 308 Versterker.write(1);
s1503189 0:c727f4699e80 309 // Attach the data read and send function at 100 Hz
s1503189 0:c727f4699e80 310 scopeTimer.attach_us(&tickerfunctie, 1/0.0003);// 1e4);
s1503189 0:c727f4699e80 311 ForceTimer.attach_us(&tickerfunctie2, 1e4);
s1503189 0:c727f4699e80 312 while(1) {
s1503189 0:c727f4699e80 313 while(LoopTimerFlag !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 0:c727f4699e80 314 LoopTimerFlag = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
s1503189 0:c727f4699e80 315 Led = 1;
s1503189 0:c727f4699e80 316 Y_filt = 3*Filter1.step(a_in3.read());
s1503189 0:c727f4699e80 317 Y_filt2 = Filter2.step(Y_filt);
s1503189 0:c727f4699e80 318 Input = Inputberekening3(Force_nieuw); //Inputberekening(Boardpotmeter.read()); //Input2(Y_filtder); //Inputberekening(Boardpotmeter.read()); Input3(Force_nieuw);
s1503189 0:c727f4699e80 319 time_ += time_increment;
s1503189 0:c727f4699e80 320 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 321 Error = Errorberekening(Ref2, Input);
s1503189 0:c727f4699e80 322 Ref_der = Referentie2-Ref_prev;
s1503189 0:c727f4699e80 323 Input_der = Input-Input_prev;
s1503189 0:c727f4699e80 324 Ref_prev = Referentie2;
s1503189 0:c727f4699e80 325 Input_prev = Input;
s1503189 0:c727f4699e80 326 Output = PID_controller(Error,3,2,1,0.01, Error_prev, Error_int);
s1503189 0:c727f4699e80 327 Motor_controller();
s1503189 0:c727f4699e80 328 if (LoopTimerFlag2==1) {
s1503189 0:c727f4699e80 329 Afgeleide_Force();
s1503189 0:c727f4699e80 330 Afgeleide_Plaats();
s1503189 0:c727f4699e80 331 Force_nieuw = Y_filtder-Boardpotmeter2.read()*Plaats_der;
s1503189 0:c727f4699e80 332 }
s1503189 0:c727f4699e80 333 scopeSend();
s1503189 0:c727f4699e80 334 }
s1503189 0:c727f4699e80 335
s1503189 0:c727f4699e80 336
s1503189 0:c727f4699e80 337 }