Gerber Loman / Mbed 2 deprecated MBR_Script_Group_20

Script of MBR Group 20. Control of robot by EMG and/or potmeters

Dependencies:   Encoder HIDScope MODSERIAL biquadFilter mbed

Fork of Script_Group_20 by Gerber Loman

main.cpp@17:dbdbd1edc260, 2017-11-02 (annotated)

Committer:
charloverwijk
Date:
Thu Nov 02 22:32:42 2017 +0000
Revision:
17:dbdbd1edc260
Parent:
16:2f89d6e25782
Child:
18:1e4f697a92cb
plus knipperlicht

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Miriam 0:d5fb29bc0847 1 //libaries
Miriam 0:d5fb29bc0847 2 #include "mbed.h"
Miriam 0:d5fb29bc0847 3 #include "BiQuad.h"
Miriam 0:d5fb29bc0847 4 #include "HIDScope.h"
Miriam 0:d5fb29bc0847 5 #include "encoder.h"
Miriam 0:d5fb29bc0847 6 #include "MODSERIAL.h"
Miriam 0:d5fb29bc0847 7
paulineoonk 15:1cfe58aea10d 8 //State Machine en calibratie
paulineoonk 15:1cfe58aea10d 9 enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo};
paulineoonk 15:1cfe58aea10d 10 States State;
paulineoonk 15:1cfe58aea10d 11 int Counter;
paulineoonk 14:a861ba49107c 12 bool Position_controller_on;
paulineoonk 15:1cfe58aea10d 13 double looptime = 0.002f;
paulineoonk 15:1cfe58aea10d 14 double value;
paulineoonk 15:1cfe58aea10d 15 double home1;
paulineoonk 15:1cfe58aea10d 16 DigitalIn button (D4);
paulineoonk 15:1cfe58aea10d 17
paulineoonk 15:1cfe58aea10d 18 //Encoder/motor
paulineoonk 15:1cfe58aea10d 19 double Huidigepositie1;
paulineoonk 15:1cfe58aea10d 20 double Huidigepositie2;
paulineoonk 15:1cfe58aea10d 21 double motorValue1;
paulineoonk 15:1cfe58aea10d 22 double motorValue2;
paulineoonk 14:a861ba49107c 23
paulineoonk 3:36e706d6b3d2 24 //globalvariables Motor
paulineoonk 3:36e706d6b3d2 25 Ticker Treecko; //We make a awesome ticker for our control system
paulineoonk 8:c4ec359af35d 26 Ticker printer;
paulineoonk 16:2f89d6e25782 27 PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed
paulineoonk 16:2f89d6e25782 28 PwmOut M2E(D5);
paulineoonk 16:2f89d6e25782 29 DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control
paulineoonk 16:2f89d6e25782 30 Encoder motor1(D13,D12,true);
paulineoonk 16:2f89d6e25782 31 Encoder motor2(D9,D8,true);
paulineoonk 16:2f89d6e25782 32 DigitalOut M2D(D4);
Miriam 0:d5fb29bc0847 33
paulineoonk 16:2f89d6e25782 34 //DEMO
paulineoonk 16:2f89d6e25782 35 AnalogIn potMeter2(A1);
paulineoonk 16:2f89d6e25782 36 AnalogIn potMeter1(A2);
paulineoonk 16:2f89d6e25782 37
paulineoonk 16:2f89d6e25782 38
paulineoonk 16:2f89d6e25782 39
paulineoonk 3:36e706d6b3d2 40 MODSERIAL pc(USBTX,USBRX);
paulineoonk 3:36e706d6b3d2 41
paulineoonk 16:2f89d6e25782 42 double PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
paulineoonk 3:36e706d6b3d2 43 const double Ts = 0.1; // tickettijd/ sample time
paulineoonk 16:2f89d6e25782 44 double e_prev = 0;
paulineoonk 16:2f89d6e25782 45 double e_int = 0;
paulineoonk 16:2f89d6e25782 46 double e_prev2 = 0;
paulineoonk 16:2f89d6e25782 47 double e_int2 = 0;
paulineoonk 16:2f89d6e25782 48
paulineoonk 6:e0e5da2c068f 49 double tijdstap = 0.002;
paulineoonk 8:c4ec359af35d 50 volatile double LBF;
paulineoonk 8:c4ec359af35d 51 volatile double RBF;
paulineoonk 8:c4ec359af35d 52 volatile double LTF;
paulineoonk 8:c4ec359af35d 53 volatile double RTF;
Miriam 0:d5fb29bc0847 54
paulineoonk 7:05c71a859d27 55 //buttons en leds voor calibration
paulineoonk 7:05c71a859d27 56 DigitalIn button1(PTA4);
paulineoonk 9:285499f48cdd 57
charloverwijk 17:dbdbd1edc260 58 DigitalOut led(D2)
paulineoonk 8:c4ec359af35d 59
paulineoonk 10:518a8617c86e 60 //MVC for calibration
paulineoonk 10:518a8617c86e 61 double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
paulineoonk 10:518a8617c86e 62 //MEAN for calibration - rest
paulineoonk 10:518a8617c86e 63 double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;
paulineoonk 9:285499f48cdd 64
paulineoonk 10:518a8617c86e 65 double emgMEANSUBLB;double emgMEANSUBRB ;double emgMEANSUBLT ;double emgMEANSUBRT ;
paulineoonk 10:518a8617c86e 66 double emgSUMLB;double emgSUMRB;double emgSUMLT;double emgSUMRT;
paulineoonk 10:518a8617c86e 67
paulineoonk 8:c4ec359af35d 68
paulineoonk 7:05c71a859d27 69 bool caldone = false;
paulineoonk 8:c4ec359af35d 70 int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
paulineoonk 8:c4ec359af35d 71
paulineoonk 9:285499f48cdd 72 int Timescalibration = 0;
paulineoonk 10:518a8617c86e 73 int TimescalibrationREST = 0;
paulineoonk 10:518a8617c86e 74
Miriam 0:d5fb29bc0847 75
paulineoonk 9:285499f48cdd 76 // Biquad filters voor Left Bicep (LB)
paulineoonk 9:285499f48cdd 77 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 78 BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 79 BiQuadChain NFLB;
paulineoonk 9:285499f48cdd 80 BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 81 BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 82 BiQuadChain HPFLB;
paulineoonk 9:285499f48cdd 83 BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 84 BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 85 BiQuadChain LPFLB;
paulineoonk 9:285499f48cdd 86
paulineoonk 9:285499f48cdd 87 // Biquad filters voor Right Bicep (RB)
Miriam 0:d5fb29bc0847 88 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 89 BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 90 BiQuadChain NFRB;
paulineoonk 9:285499f48cdd 91 BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 92 BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 93 BiQuadChain HPFRB;
paulineoonk 9:285499f48cdd 94 BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 95 BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 96 BiQuadChain LPFRB;
paulineoonk 9:285499f48cdd 97
paulineoonk 9:285499f48cdd 98 // Biquad filters voor Left Tricep (LT)
paulineoonk 9:285499f48cdd 99 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 100 BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 101 BiQuadChain NFLT;
paulineoonk 9:285499f48cdd 102 BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 103 BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 104 BiQuadChain HPFLT;
paulineoonk 9:285499f48cdd 105 BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 106 BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 107 BiQuadChain LPFLT;
paulineoonk 9:285499f48cdd 108
paulineoonk 9:285499f48cdd 109 // Biquad filters voor Left Tricep (RT)
paulineoonk 9:285499f48cdd 110 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 111 BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 112 BiQuadChain NFRT;
paulineoonk 9:285499f48cdd 113 BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 114 BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 115 BiQuadChain HPFRT;
paulineoonk 9:285499f48cdd 116 BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 117 BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 118 BiQuadChain LPFRT;
paulineoonk 9:285499f48cdd 119
paulineoonk 15:1cfe58aea10d 120 Timer LooptimeEMG; //moetuiteindelijk weg
paulineoonk 6:e0e5da2c068f 121
paulineoonk 7:05c71a859d27 122 //filters
paulineoonk 8:c4ec359af35d 123 double emgNotchLB;
paulineoonk 8:c4ec359af35d 124 double emgHPLB;
paulineoonk 8:c4ec359af35d 125 double emgAbsHPLB;
paulineoonk 8:c4ec359af35d 126 double emgLPLB;
paulineoonk 8:c4ec359af35d 127
paulineoonk 8:c4ec359af35d 128 double emgNotchRB;
paulineoonk 8:c4ec359af35d 129 double emgHPRB;
paulineoonk 8:c4ec359af35d 130 double emgAbsHPRB;
paulineoonk 8:c4ec359af35d 131 double emgLPRB;
paulineoonk 7:05c71a859d27 132
paulineoonk 8:c4ec359af35d 133 double emgNotchLT;
paulineoonk 8:c4ec359af35d 134 double emgHPLT;
paulineoonk 8:c4ec359af35d 135 double emgAbsHPLT;
paulineoonk 8:c4ec359af35d 136 double emgLPLT;
paulineoonk 7:05c71a859d27 137
paulineoonk 8:c4ec359af35d 138 double emgNotchRT;
paulineoonk 8:c4ec359af35d 139 double emgHPRT;
paulineoonk 8:c4ec359af35d 140 double emgAbsHPRT;
paulineoonk 8:c4ec359af35d 141 double emgLPRT;
Miriam 0:d5fb29bc0847 142
paulineoonk 3:36e706d6b3d2 143 double f = 500; // frequency
paulineoonk 3:36e706d6b3d2 144 double dt = 1/f; // sample frequency
paulineoonk 8:c4ec359af35d 145
paulineoonk 8:c4ec359af35d 146 AnalogIn emgLB(A0); // EMG lezen
paulineoonk 8:c4ec359af35d 147 AnalogIn emgRB(A1);
paulineoonk 8:c4ec359af35d 148 AnalogIn emgLT(A2);
paulineoonk 8:c4ec359af35d 149 AnalogIn emgRT(A3);
Miriam 0:d5fb29bc0847 150
paulineoonk 16:2f89d6e25782 151 //double MVCLB = 0.3;
paulineoonk 16:2f89d6e25782 152 //double MVCRB = 0.3;
paulineoonk 16:2f89d6e25782 153 //double MVCLT = 0.3;
paulineoonk 16:2f89d6e25782 154 //double MVCRT = 0.3;
paulineoonk 8:c4ec359af35d 155
paulineoonk 11:b46a4c48c08f 156 // variabelen changePosition
paulineoonk 11:b46a4c48c08f 157 int goalx, goaly;
paulineoonk 11:b46a4c48c08f 158
paulineoonk 8:c4ec359af35d 159 void Filteren()
Miriam 0:d5fb29bc0847 160 {
paulineoonk 15:1cfe58aea10d 161 // LooptimeEMG.reset();
paulineoonk 15:1cfe58aea10d 162 // LooptimeEMG.start();
paulineoonk 8:c4ec359af35d 163
paulineoonk 8:c4ec359af35d 164 //EMG 1
paulineoonk 8:c4ec359af35d 165
paulineoonk 10:518a8617c86e 166 emgNotchLB = NFLB.step(emgLB.read() ); // Notch filter
paulineoonk 10:518a8617c86e 167 emgHPLB = HPFLB.step(emgNotchLB); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 168 emgAbsHPLB = abs(emgHPLB); // Take absolute value
paulineoonk 10:518a8617c86e 169 emgLPLB = LPFLB.step(emgAbsHPLB); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 170 emgMEANSUBLB = emgLPLB - RESTMEANLB; //substract the restmean value
paulineoonk 10:518a8617c86e 171 LBF = emgLPLB/MVCLB; // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
paulineoonk 9:285499f48cdd 172
paulineoonk 10:518a8617c86e 173 emgNotchRB = NFRB.step(emgRB.read()); // Notch filter
paulineoonk 10:518a8617c86e 174 emgHPRB = HPFRB.step(emgNotchRB); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 175 emgAbsHPRB = abs(emgHPRB); // Take absolute value
paulineoonk 10:518a8617c86e 176 emgLPRB = LPFRB.step(emgAbsHPRB); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 177 emgMEANSUBLB = emgLPLB - RESTMEANLB;
paulineoonk 9:285499f48cdd 178 RBF = emgLPRB/MVCRB; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 179
paulineoonk 10:518a8617c86e 180 emgNotchLT = NFLT.step(emgLT.read() ); // Notch filter
paulineoonk 10:518a8617c86e 181 emgHPLT = HPFLT.step(emgNotchLT); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 182 emgAbsHPLT = abs(emgHPLT); // Take absolute value
paulineoonk 10:518a8617c86e 183 emgLPLT = LPFLT.step(emgAbsHPLT); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 184 emgMEANSUBLT = emgLPLT - RESTMEANLT; //substract the restmean value
paulineoonk 9:285499f48cdd 185 LTF = emgLPLT/MVCLT; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 186
paulineoonk 10:518a8617c86e 187 emgNotchRT = NFRT.step(emgRT.read() ); // Notch filter
paulineoonk 10:518a8617c86e 188 emgHPRT = HPFRT.step(emgNotchRT); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 189 emgAbsHPRT = abs(emgHPRT); // Take absolute value
paulineoonk 10:518a8617c86e 190 emgLPRT = LPFRT.step(emgAbsHPRT); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 191 emgMEANSUBRT = emgLPRT - RESTMEANRT; //substract the restmean value
paulineoonk 9:285499f48cdd 192 RTF = emgLPRT/MVCRT; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 193
paulineoonk 8:c4ec359af35d 194 //if (emgFiltered >1)
paulineoonk 8:c4ec359af35d 195 //{
paulineoonk 8:c4ec359af35d 196 // emgFiltered=1.00;
paulineoonk 8:c4ec359af35d 197 //}
paulineoonk 8:c4ec359af35d 198 //pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n, emgreadRB = %f , emgFiltered = %f, maxi = %f \r\n, emgreadLT = %f , emgFiltered = %f, maxi = %f \r\n, emgreadRT = %f , emgFiltered = %f, maxi = %f \r\n",emgLB.read(), LBF, maxiLB,looptime.read(),emgRB.read(), RBF, maxiRB,emgLT.read(), LTF, maxiLT, emgRT.read(), RTF, maxiRT);
paulineoonk 6:e0e5da2c068f 199 //int maxwaarde = 4096; // = 64x64
paulineoonk 6:e0e5da2c068f 200 //double refP = emgFiltered*maxwaarde;
paulineoonk 6:e0e5da2c068f 201 //return refP; // value between 0 and 4096
paulineoonk 9:285499f48cdd 202
Miriam 0:d5fb29bc0847 203 }
paulineoonk 9:285499f48cdd 204
paulineoonk 10:518a8617c86e 205 void CalibrationEMG()
paulineoonk 7:05c71a859d27 206 {
paulineoonk 7:05c71a859d27 207 Timescalibration++;
paulineoonk 9:285499f48cdd 208
paulineoonk 10:518a8617c86e 209 if(Timescalibration<2000)
paulineoonk 9:285499f48cdd 210 {
charloverwijk 17:dbdbd1edc260 211 led = 1;
charloverwijk 17:dbdbd1edc260 212 wait(0.2);
charloverwijk 17:dbdbd1edc260 213 led = 0;
paulineoonk 10:518a8617c86e 214
paulineoonk 10:518a8617c86e 215 emgNotchLB = NFLB.step(emgLB.read() );
paulineoonk 10:518a8617c86e 216 emgHPLB = HPFLB.step(emgNotchLB);
paulineoonk 10:518a8617c86e 217 emgAbsHPLB = abs(emgHPLB);
paulineoonk 10:518a8617c86e 218 emgLPLB = LPFLB.step(emgAbsHPLB);
paulineoonk 10:518a8617c86e 219 emgSUMLB += emgLPLB; //SUM all rest values LB
paulineoonk 10:518a8617c86e 220
paulineoonk 11:b46a4c48c08f 221 emgNotchRB = NFRB.step(emgRB.read());
paulineoonk 10:518a8617c86e 222 emgHPRB = HPFRB.step(emgNotchRB);
paulineoonk 10:518a8617c86e 223 emgAbsHPRB = abs(emgHPRB);
paulineoonk 10:518a8617c86e 224 emgLPRB = LPFRB.step(emgAbsHPRB);
paulineoonk 10:518a8617c86e 225 emgSUMRB += emgLPRB; //SUM all rest values RB
paulineoonk 10:518a8617c86e 226
paulineoonk 10:518a8617c86e 227 emgNotchLT = NFLT.step(emgLT.read() );
paulineoonk 10:518a8617c86e 228 emgHPLT = HPFLT.step(emgNotchLT);
paulineoonk 10:518a8617c86e 229 emgAbsHPLT = abs(emgHPLT);
paulineoonk 10:518a8617c86e 230 emgLPLT = LPFLT.step(emgAbsHPLT);
paulineoonk 10:518a8617c86e 231 emgSUMLT += emgLPLT; //SUM all rest values LT
paulineoonk 10:518a8617c86e 232
paulineoonk 10:518a8617c86e 233 emgNotchRT = NFRT.step(emgRT.read() );
paulineoonk 10:518a8617c86e 234 emgHPRT = HPFRT.step(emgNotchRT);
paulineoonk 10:518a8617c86e 235 emgAbsHPRT = abs(emgHPRT);
paulineoonk 10:518a8617c86e 236 emgLPRT = LPFRT.step(emgAbsHPRT);
paulineoonk 10:518a8617c86e 237 emgSUMRT += emgLPRT; //SUM all rest values RT
paulineoonk 10:518a8617c86e 238 }
paulineoonk 10:518a8617c86e 239 if(Timescalibration==1999)
paulineoonk 10:518a8617c86e 240 {
charloverwijk 17:dbdbd1edc260 241 led = 1;
charloverwijk 17:dbdbd1edc260 242 wait(0.2);
charloverwijk 17:dbdbd1edc260 243 led = 0;
charloverwijk 17:dbdbd1edc260 244 wait(0.2);
charloverwijk 17:dbdbd1edc260 245 led = 1;
charloverwijk 17:dbdbd1edc260 246 wait(0.2);
charloverwijk 17:dbdbd1edc260 247 led = 0;
paulineoonk 10:518a8617c86e 248 RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 249 RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 250 RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 251 RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 252 }
paulineoonk 10:518a8617c86e 253 if(Timescalibration>2000 && Timescalibration<6000)
paulineoonk 10:518a8617c86e 254 {
charloverwijk 17:dbdbd1edc260 255 led = 1;
charloverwijk 17:dbdbd1edc260 256 wait(0.2);
charloverwijk 17:dbdbd1edc260 257 led = 0;
charloverwijk 17:dbdbd1edc260 258 wait(0.2);
charloverwijk 17:dbdbd1edc260 259 led = 1;
charloverwijk 17:dbdbd1edc260 260 wait(0.2);
charloverwijk 17:dbdbd1edc260 261 led = 0;
charloverwijk 17:dbdbd1edc260 262 wait(0.2);
charloverwijk 17:dbdbd1edc260 263 led = 1;
charloverwijk 17:dbdbd1edc260 264 wait(0.2);
charloverwijk 17:dbdbd1edc260 265 led = 0;
paulineoonk 10:518a8617c86e 266 emgNotchLB = NFLB.step(emgLB.read() );
paulineoonk 10:518a8617c86e 267 emgHPLB = HPFLB.step(emgNotchLB);
paulineoonk 10:518a8617c86e 268 emgAbsHPLB = abs(emgHPLB);
paulineoonk 10:518a8617c86e 269 emgLPLB = LPFLB.step(emgAbsHPLB);
paulineoonk 9:285499f48cdd 270 double emgfinalLB = emgLPLB;
paulineoonk 9:285499f48cdd 271 if (emgfinalLB > MVCLB)
paulineoonk 9:285499f48cdd 272 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 273 MVCLB = emgfinalLB;
paulineoonk 9:285499f48cdd 274 }
paulineoonk 9:285499f48cdd 275 }
paulineoonk 9:285499f48cdd 276
paulineoonk 10:518a8617c86e 277 if(Timescalibration>6000 && Timescalibration<10000)
paulineoonk 9:285499f48cdd 278 {
charloverwijk 17:dbdbd1edc260 279 led = 1;
charloverwijk 17:dbdbd1edc260 280 wait(0.2);
charloverwijk 17:dbdbd1edc260 281 led = 0;
charloverwijk 17:dbdbd1edc260 282 wait(0.2);
charloverwijk 17:dbdbd1edc260 283 led = 1;
charloverwijk 17:dbdbd1edc260 284 wait(0.2);
charloverwijk 17:dbdbd1edc260 285 led = 0;
charloverwijk 17:dbdbd1edc260 286 wait(0.2);
charloverwijk 17:dbdbd1edc260 287 led = 1;
charloverwijk 17:dbdbd1edc260 288 wait(0.2);
charloverwijk 17:dbdbd1edc260 289 led = 0;
charloverwijk 17:dbdbd1edc260 290 wait(0.2);
charloverwijk 17:dbdbd1edc260 291 led = 1;
charloverwijk 17:dbdbd1edc260 292 wait(0.2);
charloverwijk 17:dbdbd1edc260 293 led = 0;
paulineoonk 10:518a8617c86e 294 emgNotchRB = NFRB.step(emgRB.read());
paulineoonk 10:518a8617c86e 295 emgHPRB = HPFRB.step(emgNotchRB);
paulineoonk 10:518a8617c86e 296 emgAbsHPRB = abs(emgHPRB);
paulineoonk 10:518a8617c86e 297 emgLPRB = LPFRB.step(emgAbsHPRB);
paulineoonk 9:285499f48cdd 298 double emgfinalRB = emgLPRB;
paulineoonk 9:285499f48cdd 299 if (emgfinalRB > MVCRB)
paulineoonk 7:05c71a859d27 300 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 301 MVCRB = emgfinalRB;
paulineoonk 9:285499f48cdd 302 }
paulineoonk 9:285499f48cdd 303 }
paulineoonk 9:285499f48cdd 304
paulineoonk 10:518a8617c86e 305 if(Timescalibration>10000 && Timescalibration<14000)
paulineoonk 9:285499f48cdd 306 {
charloverwijk 17:dbdbd1edc260 307 led = 1;
charloverwijk 17:dbdbd1edc260 308 wait(0.2);
charloverwijk 17:dbdbd1edc260 309 led = 0;
charloverwijk 17:dbdbd1edc260 310 wait(0.2);
charloverwijk 17:dbdbd1edc260 311 led = 1;
charloverwijk 17:dbdbd1edc260 312 wait(0.2);
charloverwijk 17:dbdbd1edc260 313 led = 0;
charloverwijk 17:dbdbd1edc260 314 wait(0.2);
charloverwijk 17:dbdbd1edc260 315 led = 1;
charloverwijk 17:dbdbd1edc260 316 wait(0.2);
charloverwijk 17:dbdbd1edc260 317 led = 0;
charloverwijk 17:dbdbd1edc260 318 wait(0.2);
charloverwijk 17:dbdbd1edc260 319 led = 1;
charloverwijk 17:dbdbd1edc260 320 wait(0.2);
charloverwijk 17:dbdbd1edc260 321 led = 0;
charloverwijk 17:dbdbd1edc260 322 wait(0.2);
charloverwijk 17:dbdbd1edc260 323 led = 1;
charloverwijk 17:dbdbd1edc260 324 wait(0.2);
charloverwijk 17:dbdbd1edc260 325 led = 0;
paulineoonk 10:518a8617c86e 326 emgNotchLT = NFLT.step(emgLT.read() );
paulineoonk 10:518a8617c86e 327 emgHPLT = HPFLT.step(emgNotchLT);
paulineoonk 10:518a8617c86e 328 emgAbsHPLT = abs(emgHPLT);
paulineoonk 10:518a8617c86e 329 emgLPLT = LPFLT.step(emgAbsHPLT);
paulineoonk 9:285499f48cdd 330 double emgfinalLT = emgLPLT;
paulineoonk 9:285499f48cdd 331 if (emgfinalLT > MVCLT)
paulineoonk 9:285499f48cdd 332 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 333 MVCLT = emgfinalLT;
paulineoonk 7:05c71a859d27 334 }
paulineoonk 9:285499f48cdd 335 }
paulineoonk 9:285499f48cdd 336
paulineoonk 10:518a8617c86e 337 if(Timescalibration>14000 && Timescalibration<18000)
paulineoonk 9:285499f48cdd 338 {
paulineoonk 10:518a8617c86e 339 emgNotchRT = NFRT.step(emgRT.read() );
paulineoonk 10:518a8617c86e 340 emgHPRT = HPFRT.step(emgNotchRT);
paulineoonk 10:518a8617c86e 341 emgAbsHPRT = abs(emgHPRT);
paulineoonk 10:518a8617c86e 342 emgLPRT = LPFRT.step(emgAbsHPRT);
paulineoonk 9:285499f48cdd 343 double emgfinalRT = emgLPRT;
paulineoonk 9:285499f48cdd 344 if (emgfinalRT > MVCRT)
paulineoonk 9:285499f48cdd 345 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 346 MVCRT = emgfinalRT;
paulineoonk 9:285499f48cdd 347 }
paulineoonk 9:285499f48cdd 348 }
paulineoonk 9:285499f48cdd 349
paulineoonk 10:518a8617c86e 350 if(Timescalibration>18000)
paulineoonk 7:05c71a859d27 351 {
paulineoonk 7:05c71a859d27 352 caldone=true;
paulineoonk 7:05c71a859d27 353 }
paulineoonk 9:285499f48cdd 354 // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
paulineoonk 9:285499f48cdd 355 //}
paulineoonk 7:05c71a859d27 356 //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
paulineoonk 7:05c71a859d27 357 //return maxi;
paulineoonk 7:05c71a859d27 358 }
paulineoonk 16:2f89d6e25782 359
paulineoonk 16:2f89d6e25782 360 double GetReferencePosition()
paulineoonk 16:2f89d6e25782 361 {
paulineoonk 16:2f89d6e25782 362 double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
paulineoonk 16:2f89d6e25782 363 int maxwaarde = 4096; // = 64x64
paulineoonk 16:2f89d6e25782 364 double refP = Potmeterwaarde*maxwaarde;
paulineoonk 16:2f89d6e25782 365 return refP; // value between 0 and 4096
paulineoonk 16:2f89d6e25782 366 }
paulineoonk 16:2f89d6e25782 367
paulineoonk 16:2f89d6e25782 368 double GetReferencePosition2()
paulineoonk 16:2f89d6e25782 369 {
paulineoonk 16:2f89d6e25782 370 double potmeterwaarde2 = potMeter1.read();
paulineoonk 16:2f89d6e25782 371 int maxwaarde2 = 4096; // = 64x64
paulineoonk 16:2f89d6e25782 372 double refP2 = potmeterwaarde2*maxwaarde2;
paulineoonk 16:2f89d6e25782 373 return refP2; // value between 0 and 4096
paulineoonk 16:2f89d6e25782 374 }
paulineoonk 16:2f89d6e25782 375
paulineoonk 16:2f89d6e25782 376 double FeedBackControl(double error, double &e_prev, double &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
paulineoonk 16:2f89d6e25782 377 {
paulineoonk 16:2f89d6e25782 378 double kp = 0.001; // kind of scaled.
paulineoonk 16:2f89d6e25782 379 double Proportional= kp*error;
paulineoonk 16:2f89d6e25782 380
paulineoonk 16:2f89d6e25782 381 double kd = 0.0004; // kind of scaled.
paulineoonk 16:2f89d6e25782 382 double VelocityError = (error - e_prev)/Ts;
paulineoonk 16:2f89d6e25782 383 double Derivative = kd*VelocityError;
paulineoonk 16:2f89d6e25782 384 e_prev = error;
paulineoonk 16:2f89d6e25782 385
paulineoonk 16:2f89d6e25782 386 double ki = 0.00005; // kind of scaled.
paulineoonk 16:2f89d6e25782 387 e_int = e_int+Ts*error;
paulineoonk 16:2f89d6e25782 388 double Integrator = ki*e_int;
paulineoonk 16:2f89d6e25782 389
paulineoonk 16:2f89d6e25782 390 double motorValue = Proportional + Integrator + Derivative;
paulineoonk 16:2f89d6e25782 391 return motorValue;
paulineoonk 16:2f89d6e25782 392 }
paulineoonk 16:2f89d6e25782 393
paulineoonk 16:2f89d6e25782 394 double FeedBackControl2(double error2, double &e_prev2, double &e_int2) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
paulineoonk 16:2f89d6e25782 395 {
paulineoonk 16:2f89d6e25782 396 double kp2 = 0.001; // kind of scaled.
paulineoonk 16:2f89d6e25782 397 double Proportional2= kp2*error2;
paulineoonk 16:2f89d6e25782 398
paulineoonk 16:2f89d6e25782 399 double kd2 = 0.0004; // kind of scaled.
paulineoonk 16:2f89d6e25782 400 double VelocityError2 = (error2 - e_prev2)/Ts;
paulineoonk 16:2f89d6e25782 401 double Derivative2 = kd2*VelocityError2;
paulineoonk 16:2f89d6e25782 402 e_prev2 = error2;
paulineoonk 16:2f89d6e25782 403
paulineoonk 16:2f89d6e25782 404 double ki2 = 0.00005; // kind of scaled.
paulineoonk 16:2f89d6e25782 405 e_int2 = e_int2+Ts*error2;
paulineoonk 16:2f89d6e25782 406 double Integrator2 = ki2*e_int2;
paulineoonk 16:2f89d6e25782 407
paulineoonk 16:2f89d6e25782 408 double motorValue2 = Proportional2 + Integrator2 + Derivative2;
paulineoonk 16:2f89d6e25782 409 return motorValue2;
paulineoonk 16:2f89d6e25782 410 }
paulineoonk 16:2f89d6e25782 411
paulineoonk 16:2f89d6e25782 412
paulineoonk 16:2f89d6e25782 413 void SetMotor1(double motorValue)
paulineoonk 16:2f89d6e25782 414 {
paulineoonk 16:2f89d6e25782 415 if (motorValue >= 0) {
paulineoonk 16:2f89d6e25782 416 M1D = 0;
paulineoonk 16:2f89d6e25782 417 }
paulineoonk 16:2f89d6e25782 418 else {
paulineoonk 16:2f89d6e25782 419 M1D = 1;
paulineoonk 16:2f89d6e25782 420 }
paulineoonk 16:2f89d6e25782 421
paulineoonk 16:2f89d6e25782 422 if (fabs(motorValue) > 1) {
paulineoonk 16:2f89d6e25782 423 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
paulineoonk 16:2f89d6e25782 424 }
paulineoonk 16:2f89d6e25782 425 else {
paulineoonk 16:2f89d6e25782 426 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
paulineoonk 16:2f89d6e25782 427 }
paulineoonk 16:2f89d6e25782 428 }
paulineoonk 16:2f89d6e25782 429
paulineoonk 16:2f89d6e25782 430 void SetMotor2(double motorValue2)
paulineoonk 16:2f89d6e25782 431 {
paulineoonk 16:2f89d6e25782 432 if (motorValue2 >= 0) {
paulineoonk 16:2f89d6e25782 433 M2D = 0;
paulineoonk 16:2f89d6e25782 434 }
paulineoonk 16:2f89d6e25782 435 else {
paulineoonk 16:2f89d6e25782 436 M2D = 1;
paulineoonk 16:2f89d6e25782 437 }
paulineoonk 16:2f89d6e25782 438
paulineoonk 16:2f89d6e25782 439 if (fabs(motorValue2) > 1) {
paulineoonk 16:2f89d6e25782 440 M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
paulineoonk 16:2f89d6e25782 441 }
paulineoonk 16:2f89d6e25782 442 else {
paulineoonk 16:2f89d6e25782 443 M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
paulineoonk 16:2f89d6e25782 444 }
paulineoonk 16:2f89d6e25782 445 }
paulineoonk 16:2f89d6e25782 446
paulineoonk 16:2f89d6e25782 447 void MeasureAndControl(void)
paulineoonk 16:2f89d6e25782 448 {
paulineoonk 16:2f89d6e25782 449 // hier the control of the 1st control system
paulineoonk 16:2f89d6e25782 450 double refP = GetReferencePosition(); //KOMT UIT RKI
paulineoonk 16:2f89d6e25782 451 double Huidigepositie = motor1.getPosition();
paulineoonk 16:2f89d6e25782 452 double error = (refP - Huidigepositie);// make an error
paulineoonk 16:2f89d6e25782 453 double motorValue = FeedBackControl(error, e_prev, e_int);
paulineoonk 16:2f89d6e25782 454 SetMotor1(motorValue);
paulineoonk 16:2f89d6e25782 455 // hier the control of the 2nd control system
paulineoonk 16:2f89d6e25782 456 double refP2 = GetReferencePosition2();
paulineoonk 16:2f89d6e25782 457 double Huidigepositie2 = motor2.getPosition();
paulineoonk 16:2f89d6e25782 458 double error2 = (refP2 - Huidigepositie2);// make an error
paulineoonk 16:2f89d6e25782 459 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
paulineoonk 16:2f89d6e25782 460 SetMotor2(motorValue2);
paulineoonk 16:2f89d6e25782 461 }
paulineoonk 16:2f89d6e25782 462
paulineoonk 15:1cfe58aea10d 463 void changePosition () // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
paulineoonk 11:b46a4c48c08f 464 {
paulineoonk 11:b46a4c48c08f 465 if (RBF>0.3) {
paulineoonk 11:b46a4c48c08f 466 goalx++; // hoe veel verder gaat hij? 1 cm? 10 cm?
paulineoonk 11:b46a4c48c08f 467 }
paulineoonk 11:b46a4c48c08f 468 if (RTF>0.3) {
paulineoonk 11:b46a4c48c08f 469 goalx--;
paulineoonk 11:b46a4c48c08f 470 }
paulineoonk 11:b46a4c48c08f 471 if (LBF>0.3) {
paulineoonk 11:b46a4c48c08f 472 goaly++;
paulineoonk 11:b46a4c48c08f 473 }
paulineoonk 11:b46a4c48c08f 474 if (LTF>0.3) {
paulineoonk 11:b46a4c48c08f 475 goaly--;
paulineoonk 11:b46a4c48c08f 476 }
paulineoonk 12:65b8d29bdd5d 477 pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
paulineoonk 11:b46a4c48c08f 478 }
paulineoonk 7:05c71a859d27 479
paulineoonk 14:a861ba49107c 480 void Loop_funtion()
paulineoonk 14:a861ba49107c 481 {
paulineoonk 14:a861ba49107c 482 switch(State){
paulineoonk 14:a861ba49107c 483 case Cal1: //Calibration motor 1
paulineoonk 14:a861ba49107c 484 // naar achteren bewegen( als voorbeeld Arvid), daarna deze waarde opslaan als offset. Dan bewegen naar home middels PID en verschil encodervalue uiterste stand en home1.
paulineoonk 16:2f89d6e25782 485 motorValue1 = 0.1f; motorValue2=0;
paulineoonk 16:2f89d6e25782 486 M2E = fabs(motorValue2);
paulineoonk 16:2f89d6e25782 487 M1E = fabs(motorValue1);
paulineoonk 16:2f89d6e25782 488 if
paulineoonk 14:a861ba49107c 489 if (Huidigepositie1== 0)
paulineoonk 14:a861ba49107c 490 {
paulineoonk 16:2f89d6e25782 491 SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,.
paulineoonk 16:2f89d6e25782 492 if (fabs(Huidigepositie1-)<0.01) {
paulineoonk 15:1cfe58aea10d 493 State=Cal2;
paulineoonk 14:a861ba49107c 494 }
paulineoonk 14:a861ba49107c 495 }
paulineoonk 14:a861ba49107c 496 else {
paulineoonk 14:a861ba49107c 497 SetMotor1(0);
paulineoonk 15:1cfe58aea10d 498 Loop_funtion();
paulineoonk 14:a861ba49107c 499 }
paulineoonk 14:a861ba49107c 500 break;
paulineoonk 14:a861ba49107c 501
paulineoonk 14:a861ba49107c 502 case Cal2: //Calibration motor 2
paulineoonk 14:a861ba49107c 503 if (Huidigepositie2== 0)
paulineoonk 14:a861ba49107c 504 {
paulineoonk 15:1cfe58aea10d 505 if (Huidigepositie2<0.01){
paulineoonk 15:1cfe58aea10d 506 State=CalEMG;
paulineoonk 14:a861ba49107c 507 }
paulineoonk 14:a861ba49107c 508 else {
paulineoonk 14:a861ba49107c 509 SetMotor2(0);
paulineoonk 15:1cfe58aea10d 510 Loop_funtion();
paulineoonk 14:a861ba49107c 511 }
paulineoonk 14:a861ba49107c 512 break;
paulineoonk 14:a861ba49107c 513 case CalEMG: // Calibration EMG
paulineoonk 15:1cfe58aea10d 514 CalibrationEMG(); //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
paulineoonk 15:1cfe58aea10d 515 State=SelectDevice;
paulineoonk 14:a861ba49107c 516 break;
paulineoonk 14:a861ba49107c 517 case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
paulineoonk 15:1cfe58aea10d 518 if (button==1) {
paulineoonk 15:1cfe58aea10d 519 State=EMG;
paulineoonk 14:a861ba49107c 520 }
paulineoonk 15:1cfe58aea10d 521 if (button==0) {
paulineoonk 15:1cfe58aea10d 522 State=Demo;
paulineoonk 14:a861ba49107c 523 }
paulineoonk 14:a861ba49107c 524 break;
paulineoonk 14:a861ba49107c 525 case EMG: //Aansturen met EMG
paulineoonk 14:a861ba49107c 526 Filteren();
paulineoonk 14:a861ba49107c 527 changePosition();
paulineoonk 16:2f89d6e25782 528 //RKI --> output refP van motor
paulineoonk 16:2f89d6e25782 529 MeasureAndControl();
paulineoonk 14:a861ba49107c 530 break;
paulineoonk 15:1cfe58aea10d 531 case Demo: // Aansturen met toetsenbord
paulineoonk 14:a861ba49107c 532 break;
paulineoonk 15:1cfe58aea10d 533 }
paulineoonk 14:a861ba49107c 534 }
Miriam 0:d5fb29bc0847 535
paulineoonk 15:1cfe58aea10d 536 /*void Tickerfunctie()
paulineoonk 8:c4ec359af35d 537 {
paulineoonk 13:3351f4374885 538 //if(caldone == false)
paulineoonk 13:3351f4374885 539 //{
paulineoonk 12:65b8d29bdd5d 540 pc.printf("emgreadRB = %f , emgFiltered = %f, maxi = %f meanrest = %f\r\n",emgRB.read(), RBF, MVCRB, RESTMEANLB);
paulineoonk 11:b46a4c48c08f 541 pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, meanrest = %f emgSUMLB %f ,Timescalibration %i\r\n",emgLB.read(), LBF, MVCLB,RESTMEANRB,emgSUMLB, Timescalibration);
paulineoonk 10:518a8617c86e 542 pc.printf("emgreadRT = %f , emgFilteredRT = %f, maxiRT = %f meanrest = %f \r\n",emgRT.read(), RTF, MVCRT,RESTMEANRT);
paulineoonk 10:518a8617c86e 543 pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f meanrest = %f \r\n",emgLT.read(), LTF, MVCLT,RESTMEANLT);
paulineoonk 13:3351f4374885 544 //}
paulineoonk 8:c4ec359af35d 545 }
paulineoonk 15:1cfe58aea10d 546 */
paulineoonk 15:1cfe58aea10d 547 int main();//deze moet ooit nog weg --> pas op voor errors
Miriam 0:d5fb29bc0847 548 {
paulineoonk 7:05c71a859d27 549 //voor EMG filteren
paulineoonk 10:518a8617c86e 550 //Left Bicep
paulineoonk 9:285499f48cdd 551 NFLB.add( &N1LB );
paulineoonk 10:518a8617c86e 552 HPFLB.add( &HP1LB ).add( &HP2LB );
paulineoonk 9:285499f48cdd 553 LPFLB.add( &LP1LB ).add( &LP2LB );
paulineoonk 9:285499f48cdd 554
paulineoonk 10:518a8617c86e 555 //Right Bicep
paulineoonk 10:518a8617c86e 556 NFRB.add( &N1RB );
paulineoonk 10:518a8617c86e 557 HPFRB.add( &HP1RB ).add( &HP2RB );
paulineoonk 10:518a8617c86e 558 LPFRB.add( &LP1RB ).add( &LP2RB );
paulineoonk 9:285499f48cdd 559
paulineoonk 10:518a8617c86e 560 //Left Tricep
paulineoonk 10:518a8617c86e 561 NFLT.add( &N1LT );
paulineoonk 10:518a8617c86e 562 HPFLT.add( &HP1LT ).add( &HP2LT );
paulineoonk 10:518a8617c86e 563 LPFLT.add( &LP1LT ).add( &LP2LT );
paulineoonk 10:518a8617c86e 564
paulineoonk 10:518a8617c86e 565 //Right Tricep
paulineoonk 10:518a8617c86e 566 NFRT.add( &N1RT );
paulineoonk 10:518a8617c86e 567 HPFRT.add( &HP1RT ).add( &HP2RT );
paulineoonk 10:518a8617c86e 568 LPFRT.add( &LP1RT ).add( &LP2RT );
paulineoonk 3:36e706d6b3d2 569
paulineoonk 7:05c71a859d27 570 //voor serial
paulineoonk 7:05c71a859d27 571 pc.baud(115200);
paulineoonk 7:05c71a859d27 572
paulineoonk 7:05c71a859d27 573 //motor
paulineoonk 8:c4ec359af35d 574 // M1E.period(PwmPeriod); //set PWMposition at 5000hz
paulineoonk 7:05c71a859d27 575 //Ticker
paulineoonk 15:1cfe58aea10d 576 //Treecko.attach(MeasureAndControl, tijdstap); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
paulineoonk 3:36e706d6b3d2 577 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
paulineoonk 15:1cfe58aea10d 578 // printer.attach(Tickerfunctie,0.4);
paulineoonk 14:a861ba49107c 579
paulineoonk 14:a861ba49107c 580 //State Machine
paulineoonk 14:a861ba49107c 581 State = Cal1;
paulineoonk 14:a861ba49107c 582 Position_controller_on = false;
paulineoonk 15:1cfe58aea10d 583 Treecko.attach(&Loop_funtion, looptime);
paulineoonk 3:36e706d6b3d2 584 while(true)
paulineoonk 16:2f89d6e25782 585 { }
paulineoonk 3:36e706d6b3d2 586
paulineoonk 16:2f89d6e25782 587 //is deze wel nodig?
paulineoonk 16:2f89d6e25782 588 }