Menno Sytsma / Passief_stuurprogramma

Dit is het passieve stuurprogramma van de handorthese behorend bij mijn bacheloropdracht. Groet, Menno Sytsma

Dependencies:   EMG FastPWM HIDScope mbed-src

main.cpp@4:4ad3fc99c356, 2016-06-01 (annotated)

Committer:
s1503189
Date:
Wed Jun 01 12:44:55 2016 +0000
Revision:
4:4ad3fc99c356
Parent:
3:9f9ef68a25a2
Child:
5:f97bf30bec14
Werkende versie van State machine, met Calibratie sinus, en aantal herhalingen. Slordige versie; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1503189 0:46b6258b2b00 1 #include "mbed.h"
s1503189 0:46b6258b2b00 2 #include "FastPWM.h"
s1503189 0:46b6258b2b00 3 #include <iostream>
s1503189 0:46b6258b2b00 4 #include "HIDScope.h"
s1503189 0:46b6258b2b00 5 #include "math.h"
s1503189 0:46b6258b2b00 6 #include "biquadFilter.h"
s1503189 0:46b6258b2b00 7
s1503189 0:46b6258b2b00 8
s1503189 0:46b6258b2b00 9 DigitalOut Rood(LED_RED);
s1503189 0:46b6258b2b00 10 DigitalOut Groen(LED_GREEN);
s1503189 0:46b6258b2b00 11 DigitalOut Blauw(LED_BLUE);
s1503189 0:46b6258b2b00 12 DigitalIn Button1pressed(SW2);
s1503189 0:46b6258b2b00 13 DigitalIn Button2pressed(SW3);
s1503189 0:46b6258b2b00 14 volatile bool LoopTimerFlag = 0;
s1503189 0:46b6258b2b00 15 Serial pc(USBTX, USBRX);
s1503189 0:46b6258b2b00 16
s1503189 0:46b6258b2b00 17 int N;
s1503189 0:46b6258b2b00 18 bool Start(0);
s1503189 2:1dd9e630a7b5 19 double Setting(0);
s1503189 0:46b6258b2b00 20 bool Dotask(0);
s1503189 0:46b6258b2b00 21 int i;
s1503189 0:46b6258b2b00 22
s1503189 0:46b6258b2b00 23 Ticker Finitestatemachine;
s1503189 0:46b6258b2b00 24
s1503189 0:46b6258b2b00 25 // Verklaren van de in en outputs
s1503189 0:46b6258b2b00 26 Ticker Loopticker; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
s1503189 0:46b6258b2b00 27 volatile bool LoopTimerFlag2 = 0; // Volatile, omdat deze heel vaak verandert van waar naar onwaar.
s1503189 2:1dd9e630a7b5 28 Ticker Loopticker2; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
s1503189 2:1dd9e630a7b5 29 volatile bool LoopTimerFlag3 = 0;
s1503189 0:46b6258b2b00 30 AnalogIn Referentie(A0); // De schuifpotmeter naast de geleiding van de draad is de referentie.
s1503189 2:1dd9e630a7b5 31 AnalogIn Boardpotmeter(A2); // POT1 op het board wordt gebruikt als input van de controlloop.
s1503189 2:1dd9e630a7b5 32 AnalogIn Boardpotmeter2(A1);
s1503189 0:46b6258b2b00 33 DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
s1503189 0:46b6258b2b00 34 FastPWM motor1speed(D6);
s1503189 2:1dd9e630a7b5 35
s1503189 0:46b6258b2b00 36
s1503189 0:46b6258b2b00 37 float potmeter;
s1503189 0:46b6258b2b00 38 float Error;
s1503189 4:4ad3fc99c356 39 float Referentie2;//=0.0;
s1503189 0:46b6258b2b00 40 float Input;
s1503189 0:46b6258b2b00 41 float Output;
s1503189 2:1dd9e630a7b5 42 float POT;
s1503189 3:9f9ef68a25a2 43 float Foutje;
s1503189 3:9f9ef68a25a2 44 float Potmetertje;
s1503189 0:46b6258b2b00 45 double Error_prev = 0;
s1503189 0:46b6258b2b00 46 double Error_int = 0;
s1503189 0:46b6258b2b00 47 double Error_der;
s1503189 1:f63d8a73460c 48 double Ref_der = 0;
s1503189 1:f63d8a73460c 49 double Ref_prev = 0;
s1503189 4:4ad3fc99c356 50 float Amplitude;
s1503189 4:4ad3fc99c356 51 float Baseline;
s1503189 0:46b6258b2b00 52 // Define the HIDScope and Ticker object
s1503189 0:46b6258b2b00 53 HIDScope scope(4);
s1503189 0:46b6258b2b00 54 Ticker Hidscope;
s1503189 0:46b6258b2b00 55 // Lowpassfilter
s1503189 0:46b6258b2b00 56 const double a1_LP = -1.561018075800718, a2_LP = 0.641351538057563;
s1503189 0:46b6258b2b00 57 const double b0_LP = 0.020083365564211, b1_LP = 0.040166731128423, b2_LP = 0.020083365564211;
s1503189 0:46b6258b2b00 58 const double a1_LP2 = -1.561018075800718, a2_LP2 = 0.641351538057563;
s1503189 0:46b6258b2b00 59 const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211;
s1503189 0:46b6258b2b00 60 //const double a1_LP2 = -1.866892279711715, a2_LP2 = 0.875214548253684;
s1503189 0:46b6258b2b00 61 //const double b0_LP2 = 0.002080567135492, b1_LP2 = 0.004161134270985, b2_LP2 = 0.002080567135492;
s1503189 0:46b6258b2b00 62 //const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125;
s1503189 0:46b6258b2b00 63 //const double b0_HP = 0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;
s1503189 0:46b6258b2b00 64
s1503189 0:46b6258b2b00 65 float time_;
s1503189 0:46b6258b2b00 66 const float time_increment = 0.01;
s1503189 0:46b6258b2b00 67
s1503189 0:46b6258b2b00 68 biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);
s1503189 0:46b6258b2b00 69 biquadFilter Filter2(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);
s1503189 0:46b6258b2b00 70
s1503189 0:46b6258b2b00 71 double Referentieschaling(double A, double B)
s1503189 0:46b6258b2b00 72 {
s1503189 0:46b6258b2b00 73 double Y= 787.3008916207206*pow(A,4) -565.1143141517078*pow(A,3) + 122.8516837382677*pow(A,2) + 0.0556616744031*A + 0.0912411880277;
s1503189 0:46b6258b2b00 74 Referentie2 = Filter1.step(Y);
s1503189 0:46b6258b2b00 75 if (Referentie2<=2.5 or Referentie2>=9.2) {
s1503189 4:4ad3fc99c356 76 Rood = 0;
s1503189 0:46b6258b2b00 77 Referentie2 = B;
s1503189 0:46b6258b2b00 78 }
s1503189 0:46b6258b2b00 79 return Referentie2;
s1503189 0:46b6258b2b00 80 }
s1503189 0:46b6258b2b00 81
s1503189 0:46b6258b2b00 82 double Inputberekening(double B)
s1503189 0:46b6258b2b00 83 {
s1503189 4:4ad3fc99c356 84 Input = Baseline+(Amplitude*-cos(2*time_));
s1503189 0:46b6258b2b00 85 if (Input>=9) { // De Input moet binnnen een aantal grenzen blijven. Groter dan 7 is mechanisch niet mogelijk.
s1503189 0:46b6258b2b00 86 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:46b6258b2b00 87 } else if (Input<=3.0) {
s1503189 0:46b6258b2b00 88 Input = 3.0;
s1503189 0:46b6258b2b00 89 }
s1503189 0:46b6258b2b00 90 return Input;
s1503189 0:46b6258b2b00 91 }
s1503189 0:46b6258b2b00 92
s1503189 2:1dd9e630a7b5 93 /*double Inputberekening2(double B, double Val)
s1503189 2:1dd9e630a7b5 94 {
s1503189 2:1dd9e630a7b5 95 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 2:1dd9e630a7b5 96 Val = Filter2.step(Inp);
s1503189 2:1dd9e630a7b5 97 return Inp;
s1503189 2:1dd9e630a7b5 98 }*/
s1503189 0:46b6258b2b00 99
s1503189 0:46b6258b2b00 100 double Errorberekening(double Input,double Ref)
s1503189 0:46b6258b2b00 101 {
s1503189 0:46b6258b2b00 102 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:f63d8a73460c 103 /*if (Error>=1) {
s1503189 0:46b6258b2b00 104 Error=1;
s1503189 0:46b6258b2b00 105 } else if (Error<=-1) {
s1503189 0:46b6258b2b00 106 Error = -1;
s1503189 0:46b6258b2b00 107 } else if (fabs(Error)<0.01) {
s1503189 0:46b6258b2b00 108 Error = 0;
s1503189 0:46b6258b2b00 109 }
s1503189 1:f63d8a73460c 110 */
s1503189 0:46b6258b2b00 111 return Error;
s1503189 0:46b6258b2b00 112 }
s1503189 0:46b6258b2b00 113 double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev)
s1503189 0:46b6258b2b00 114 {
s1503189 0:46b6258b2b00 115 Error_der = (Error-Error_prev)/Ts;
s1503189 0:46b6258b2b00 116 Error_prev = Error;
s1503189 0:46b6258b2b00 117 Error_int = Error_int + Ts*Error;
s1503189 0:46b6258b2b00 118
s1503189 0:46b6258b2b00 119 return KP*Error+KI*Error_int+KD*Error_der;
s1503189 0:46b6258b2b00 120 }
s1503189 0:46b6258b2b00 121
s1503189 0:46b6258b2b00 122 // The data read and send function
s1503189 0:46b6258b2b00 123 void scopeSend()
s1503189 0:46b6258b2b00 124 {
s1503189 0:46b6258b2b00 125 scope.set(0,Referentie2); // Kanaal 1 van HIDscope geeft: De positie van de schuifpotmeter in cm.
s1503189 0:46b6258b2b00 126 scope.set(1,Input); // Kanaal 2 van HIDscope geeft: De gewenste positie in cm door de potmeter op het board ingesteld.
s1503189 0:46b6258b2b00 127 scope.set(2,Error); // Kanaal 3 van HIDscope geeft: De waarde van de Error die de P_controller in gaat.
s1503189 0:46b6258b2b00 128 scope.set(3,motor1speed.read());// Kanaal 1 van HIDscope geeft: De snelheid van de motor, is een absolute waarde, richting wordt elders gegeven.
s1503189 0:46b6258b2b00 129 scope.send();
s1503189 0:46b6258b2b00 130
s1503189 0:46b6258b2b00 131 }
s1503189 0:46b6258b2b00 132
s1503189 0:46b6258b2b00 133 void tickerfunctie2() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
s1503189 0:46b6258b2b00 134 {
s1503189 0:46b6258b2b00 135 LoopTimerFlag2 = 1;
s1503189 0:46b6258b2b00 136 }
s1503189 0:46b6258b2b00 137
s1503189 3:9f9ef68a25a2 138 void Motor_controller(float Output) // De P_controller, door de ticker elke honderdste seconde uitgevoerd.
s1503189 0:46b6258b2b00 139 {
s1503189 1:f63d8a73460c 140 if(Ref_der < 0) {
s1503189 1:f63d8a73460c 141 if(Output>=0) {
s1503189 1:f63d8a73460c 142 motor1direction.write(1);
s1503189 1:f63d8a73460c 143 motor1speed = 0.01*fabs(Output);
s1503189 1:f63d8a73460c 144 } else if (Output<0) {
s1503189 1:f63d8a73460c 145 motor1direction.write(0);
s1503189 1:f63d8a73460c 146 motor1speed = 0.05*fabs(Output);
s1503189 0:46b6258b2b00 147 }
s1503189 0:46b6258b2b00 148 }
s1503189 3:9f9ef68a25a2 149 if(Ref_der > 0) {
s1503189 1:f63d8a73460c 150 if(Output>=0) {
s1503189 1:f63d8a73460c 151 motor1direction.write(1);
s1503189 1:f63d8a73460c 152 motor1speed = (20*fabs(Output));
s1503189 1:f63d8a73460c 153 } else if (Output<0) {
s1503189 1:f63d8a73460c 154 motor1direction.write(0);
s1503189 1:f63d8a73460c 155 motor1speed = 0.01*fabs(Output);
s1503189 0:46b6258b2b00 156 }
s1503189 0:46b6258b2b00 157 }
s1503189 3:9f9ef68a25a2 158 /* if(bool Banaan = 1) {
s1503189 3:9f9ef68a25a2 159 //pc.printf("Banaan \n");
s1503189 3:9f9ef68a25a2 160 if(Output>=0) {
s1503189 3:9f9ef68a25a2 161 motor1direction.write(1);
s1503189 3:9f9ef68a25a2 162 motor1speed = fabs(Output);
s1503189 3:9f9ef68a25a2 163 } else if (Output<0) {
s1503189 3:9f9ef68a25a2 164 motor1direction.write(0);
s1503189 3:9f9ef68a25a2 165 motor1speed = fabs(Output);
s1503189 3:9f9ef68a25a2 166 }
s1503189 3:9f9ef68a25a2 167 }*/
s1503189 0:46b6258b2b00 168 }
s1503189 3:9f9ef68a25a2 169
s1503189 3:9f9ef68a25a2 170 void Motor_controller2(float Foutje)
s1503189 3:9f9ef68a25a2 171 {
s1503189 3:9f9ef68a25a2 172 if(Foutje>=0.01){
s1503189 3:9f9ef68a25a2 173 motor1direction.write(1);
s1503189 3:9f9ef68a25a2 174 motor1speed.write(0.05*Foutje);
s1503189 3:9f9ef68a25a2 175 }
s1503189 3:9f9ef68a25a2 176 else if(Foutje<=-0.01){
s1503189 3:9f9ef68a25a2 177 motor1direction.write(0);
s1503189 3:9f9ef68a25a2 178 motor1speed.write(-Foutje);
s1503189 3:9f9ef68a25a2 179 }
s1503189 3:9f9ef68a25a2 180 else {
s1503189 3:9f9ef68a25a2 181 motor1speed.write(0);
s1503189 3:9f9ef68a25a2 182 }
s1503189 3:9f9ef68a25a2 183 }
s1503189 3:9f9ef68a25a2 184
s1503189 2:1dd9e630a7b5 185 void Extendfinger()
s1503189 2:1dd9e630a7b5 186 {
s1503189 2:1dd9e630a7b5 187 Error = 0;
s1503189 2:1dd9e630a7b5 188 int J = 0;
s1503189 2:1dd9e630a7b5 189 time_ = 0; // Voordat het hele programma begint, staat de Error op 0, zodat de motor niet spastisch gaat draaien om dit te compenseren.
s1503189 3:9f9ef68a25a2 190 //while(1 && J<=320) {
s1503189 3:9f9ef68a25a2 191 while(1 && J<=320) {
s1503189 2:1dd9e630a7b5 192 while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 2:1dd9e630a7b5 193 LoopTimerFlag2 = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
s1503189 2:1dd9e630a7b5 194 J= J+1;
s1503189 2:1dd9e630a7b5 195 //pc.printf(" J is %i \n",J);
s1503189 4:4ad3fc99c356 196 Rood = 1;
s1503189 2:1dd9e630a7b5 197 double Input = Inputberekening(Boardpotmeter.read());
s1503189 2:1dd9e630a7b5 198 time_ += time_increment;
s1503189 2:1dd9e630a7b5 199 double 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 2:1dd9e630a7b5 200 Ref_der = Referentie2-Ref_prev;
s1503189 2:1dd9e630a7b5 201 Ref_prev = Referentie2;
s1503189 2:1dd9e630a7b5 202 Error = Errorberekening(Input, Referentie2);
s1503189 2:1dd9e630a7b5 203 Output = PID_controller(Error,5,1,0.1,0.01, Error_prev, Error_int);
s1503189 3:9f9ef68a25a2 204 Motor_controller(Output);
s1503189 1:f63d8a73460c 205 }
s1503189 3:9f9ef68a25a2 206 //}
s1503189 2:1dd9e630a7b5 207 motor1speed.write(0);
s1503189 1:f63d8a73460c 208
s1503189 2:1dd9e630a7b5 209 }
s1503189 1:f63d8a73460c 210
s1503189 2:1dd9e630a7b5 211 void tickerfunctie()
s1503189 2:1dd9e630a7b5 212 {
s1503189 2:1dd9e630a7b5 213 LoopTimerFlag = 1;
s1503189 2:1dd9e630a7b5 214 }
s1503189 2:1dd9e630a7b5 215
s1503189 2:1dd9e630a7b5 216 /*void Determinemax()
s1503189 2:1dd9e630a7b5 217 {
s1503189 2:1dd9e630a7b5 218 double Val = 10*Boardpotmeter.read(); // 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 2:1dd9e630a7b5 219 double Max = Filter2.step(Val);
s1503189 2:1dd9e630a7b5 220 double Set_Error = Referentie.read()-Max;
s1503189 2:1dd9e630a7b5 221 }*/
s1503189 1:f63d8a73460c 222
s1503189 3:9f9ef68a25a2 223 void P_controller(double Max, bool Keuze)
s1503189 2:1dd9e630a7b5 224 {
s1503189 3:9f9ef68a25a2 225 while (Button1pressed.read()==1) {
s1503189 3:9f9ef68a25a2 226 while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 2:1dd9e630a7b5 227 LoopTimerFlag2 = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
s1503189 2:1dd9e630a7b5 228 if (bool (0)) {
s1503189 2:1dd9e630a7b5 229 POT = Boardpotmeter.read();
s1503189 2:1dd9e630a7b5 230 } else if (bool (1)) {
s1503189 2:1dd9e630a7b5 231 POT = Boardpotmeter2.read();
s1503189 1:f63d8a73460c 232 }
s1503189 4:4ad3fc99c356 233 Input = 10*POT; //Inputberekening2(POT,0); Inputberekening(Boardpotmeter.read()); // 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 3:9f9ef68a25a2 234 // time_ += time_increment;
s1503189 3:9f9ef68a25a2 235 double 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 4:4ad3fc99c356 236 Rood = 1;
s1503189 3:9f9ef68a25a2 237 //Ref_prev = Ref2;
s1503189 4:4ad3fc99c356 238 Error = Referentie2-Input;
s1503189 3:9f9ef68a25a2 239 // pc.printf("Input = %f Referentie= %f Error= %f \n", Input, Referentie2, Error);
s1503189 3:9f9ef68a25a2 240 Motor_controller2(Error);
s1503189 3:9f9ef68a25a2 241 // pc.printf("output = %f \n", motor1speed.read());
s1503189 2:1dd9e630a7b5 242
s1503189 1:f63d8a73460c 243 }
s1503189 4:4ad3fc99c356 244
s1503189 2:1dd9e630a7b5 245 Groen = 0;
s1503189 2:1dd9e630a7b5 246 wait (0.5);
s1503189 2:1dd9e630a7b5 247 }
s1503189 2:1dd9e630a7b5 248
s1503189 2:1dd9e630a7b5 249 void Determinetask()
s1503189 2:1dd9e630a7b5 250 {
s1503189 2:1dd9e630a7b5 251 if (Button1pressed == 0) {
s1503189 2:1dd9e630a7b5 252
s1503189 2:1dd9e630a7b5 253 Groen = 0;
s1503189 2:1dd9e630a7b5 254 N++;
s1503189 1:f63d8a73460c 255 wait (0.5);
s1503189 2:1dd9e630a7b5 256 Groen = 1;
s1503189 2:1dd9e630a7b5 257 }
s1503189 2:1dd9e630a7b5 258 }
s1503189 2:1dd9e630a7b5 259
s1503189 2:1dd9e630a7b5 260 int main()
s1503189 2:1dd9e630a7b5 261 {
s1503189 2:1dd9e630a7b5 262 Rood = 1;
s1503189 2:1dd9e630a7b5 263 Blauw = 1;
s1503189 2:1dd9e630a7b5 264 Groen = 1;
s1503189 2:1dd9e630a7b5 265 int i = 0;
s1503189 2:1dd9e630a7b5 266 pc.printf("Hello World! %i \n", i);
s1503189 2:1dd9e630a7b5 267 Finitestatemachine.attach(tickerfunctie,0.1);
s1503189 3:9f9ef68a25a2 268 Loopticker.attach(tickerfunctie2,0.01);
s1503189 3:9f9ef68a25a2 269 Hidscope.attach(scopeSend,0.01); // Verzenden naar HIDscope
s1503189 2:1dd9e630a7b5 270
s1503189 2:1dd9e630a7b5 271 while (true) {
s1503189 2:1dd9e630a7b5 272 while(LoopTimerFlag !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
s1503189 2:1dd9e630a7b5 273 LoopTimerFlag = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
s1503189 2:1dd9e630a7b5 274 if (Start==0 && Setting==0 && Dotask==0) { // In eerste instantie gaan de leds even uit
s1503189 2:1dd9e630a7b5 275
s1503189 2:1dd9e630a7b5 276 Start = 1;
s1503189 2:1dd9e630a7b5 277 Groen = 0;
s1503189 2:1dd9e630a7b5 278 pc.printf("State 1\n");
s1503189 2:1dd9e630a7b5 279 }
s1503189 2:1dd9e630a7b5 280
s1503189 2:1dd9e630a7b5 281 if (Start==1 && Setting==0 && Dotask==0) { // State 2 heeft Geel als kleur
s1503189 2:1dd9e630a7b5 282 Groen = 0;
s1503189 2:1dd9e630a7b5 283 Rood = 0;
s1503189 2:1dd9e630a7b5 284 pc.printf("State 2\n");
s1503189 2:1dd9e630a7b5 285 if (Button1pressed.read()==1 && Button2pressed.read()==0) {
s1503189 2:1dd9e630a7b5 286 Setting = true;
s1503189 2:1dd9e630a7b5 287 Start = false;
s1503189 2:1dd9e630a7b5 288 Groen = 1;
s1503189 2:1dd9e630a7b5 289 Rood = 1;
s1503189 2:1dd9e630a7b5 290 pc.printf("State 2 A\n");
s1503189 2:1dd9e630a7b5 291 wait(0.5);
s1503189 2:1dd9e630a7b5 292
s1503189 1:f63d8a73460c 293
s1503189 2:1dd9e630a7b5 294 }
s1503189 2:1dd9e630a7b5 295 if (Button1pressed.read()==0 && Button2pressed.read()==1 && N>0) {
s1503189 2:1dd9e630a7b5 296 Groen = 1;
s1503189 2:1dd9e630a7b5 297 Rood = 1;
s1503189 2:1dd9e630a7b5 298 Dotask = 1;
s1503189 2:1dd9e630a7b5 299 Start=0;
s1503189 2:1dd9e630a7b5 300 pc.printf("State 2 B\n");
s1503189 2:1dd9e630a7b5 301 wait(0.5);
s1503189 2:1dd9e630a7b5 302 }
s1503189 2:1dd9e630a7b5 303 }
s1503189 3:9f9ef68a25a2 304 if (Start==0 && Setting==1 && Dotask==0) { // Leds zijn uit, elke bepaling gaat het groene lampje even branden.
s1503189 2:1dd9e630a7b5 305 pc.printf("State 2 C\n");
s1503189 3:9f9ef68a25a2 306 P_controller(0,0);
s1503189 3:9f9ef68a25a2 307 double Maximum = 10*POT;
s1503189 2:1dd9e630a7b5 308 Groen = 1;
s1503189 2:1dd9e630a7b5 309 pc.printf("Max = %f \n", Maximum);
s1503189 3:9f9ef68a25a2 310 P_controller(0,1);
s1503189 3:9f9ef68a25a2 311 double Minimum = 10*POT;
s1503189 2:1dd9e630a7b5 312 Groen = 1;
s1503189 2:1dd9e630a7b5 313 pc.printf("Min = %f \n", Minimum);
s1503189 4:4ad3fc99c356 314 Amplitude = (Minimum+Maximum)/2;
s1503189 4:4ad3fc99c356 315 Baseline = (Maximum-Minimum)/2;
s1503189 4:4ad3fc99c356 316 Rood = 0;
s1503189 4:4ad3fc99c356 317 Blauw = 0;
s1503189 4:4ad3fc99c356 318 wait (2);
s1503189 4:4ad3fc99c356 319 Rood = 1;
s1503189 4:4ad3fc99c356 320 Blauw = 1;
s1503189 2:1dd9e630a7b5 321 Setting = 2;
s1503189 2:1dd9e630a7b5 322 }
s1503189 1:f63d8a73460c 323
s1503189 2:1dd9e630a7b5 324 if (Start==0 && Setting==2 && Dotask==0) { // Leds zijn uit, elke count gaat het groene lampje even branden.
s1503189 2:1dd9e630a7b5 325 pc.printf("State 3\n");
s1503189 2:1dd9e630a7b5 326 Determinetask(); // Aantal gewenste herhalingen van Dotask instellen
s1503189 2:1dd9e630a7b5 327 pc.printf("N = %i \n", N);
s1503189 2:1dd9e630a7b5 328 if (Button1pressed.read()== 1 && Button2pressed.read()== 0) {
s1503189 2:1dd9e630a7b5 329 Setting = 0;
s1503189 2:1dd9e630a7b5 330 Dotask = 1;
s1503189 2:1dd9e630a7b5 331 pc.printf("State 3 B\n");
s1503189 2:1dd9e630a7b5 332 }
s1503189 2:1dd9e630a7b5 333 }
s1503189 1:f63d8a73460c 334
s1503189 2:1dd9e630a7b5 335 if (Start==0 && Setting==0 && Dotask==1) {
s1503189 2:1dd9e630a7b5 336 pc.printf("State 4\n");
s1503189 2:1dd9e630a7b5 337 while(i < N) {
s1503189 2:1dd9e630a7b5 338
s1503189 2:1dd9e630a7b5 339 pc.printf("i = %i N = %i \n", i, N);
s1503189 2:1dd9e630a7b5 340 Extendfinger();
s1503189 2:1dd9e630a7b5 341 //int J = 0;
s1503189 2:1dd9e630a7b5 342 pc.printf("Extendfinger()afgerond\n", i, N);
s1503189 2:1dd9e630a7b5 343 i++;
s1503189 2:1dd9e630a7b5 344 }
s1503189 2:1dd9e630a7b5 345 if (i==N) {
s1503189 2:1dd9e630a7b5 346 Dotask = 0;
s1503189 1:f63d8a73460c 347 Start = 1;
s1503189 2:1dd9e630a7b5 348 i=0;
s1503189 2:1dd9e630a7b5 349 pc.printf("Final state \n");
s1503189 1:f63d8a73460c 350 }
s1503189 1:f63d8a73460c 351
s1503189 1:f63d8a73460c 352 }
s1503189 1:f63d8a73460c 353 }
s1503189 2:1dd9e630a7b5 354 }