Projectgroep 20 Biorobotics / Mbed 2 deprecated StateMachinemooi

State Machine, bezig met mooimaken

Dependencies:   Encoder HIDScope MODSERIAL biquadFilter mbed

Fork of vanEMGnaarMOTORPauline_States_nacht by Projectgroep 20 Biorobotics

main.cpp@11:b46a4c48c08f, 2017-11-01 (annotated)

Committer:
paulineoonk
Date:
Wed Nov 01 16:18:49 2017 +0000
Revision:
11:b46a4c48c08f
Parent:
10:518a8617c86e
Child:
12:65b8d29bdd5d
werkend en "goede" printf's. Nu ook cases toegevoegd, kijken of dit werkt??; EMG kalibreren werkt in ieder geval helemaal goed!

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 3:36e706d6b3d2 8 //globalvariables Motor
paulineoonk 3:36e706d6b3d2 9 Ticker Treecko; //We make a awesome ticker for our control system
paulineoonk 8:c4ec359af35d 10 Ticker printer;
paulineoonk 8:c4ec359af35d 11 //PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed
paulineoonk 8:c4ec359af35d 12 //DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control
Miriam 0:d5fb29bc0847 13
paulineoonk 8:c4ec359af35d 14 //Encoder motor1(D13,D12,true);
paulineoonk 3:36e706d6b3d2 15 MODSERIAL pc(USBTX,USBRX);
paulineoonk 3:36e706d6b3d2 16
paulineoonk 8:c4ec359af35d 17 //double PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
paulineoonk 3:36e706d6b3d2 18 const double Ts = 0.1; // tickettijd/ sample time
paulineoonk 8:c4ec359af35d 19 //double e_prev = 0;
paulineoonk 8:c4ec359af35d 20 //double e_int = 0;
paulineoonk 6:e0e5da2c068f 21 double tijdstap = 0.002;
paulineoonk 8:c4ec359af35d 22 volatile double LBF;
paulineoonk 8:c4ec359af35d 23 volatile double RBF;
paulineoonk 8:c4ec359af35d 24 volatile double LTF;
paulineoonk 8:c4ec359af35d 25 volatile double RTF;
Miriam 0:d5fb29bc0847 26
paulineoonk 7:05c71a859d27 27 //buttons en leds voor calibration
paulineoonk 7:05c71a859d27 28 DigitalIn button1(PTA4);
paulineoonk 9:285499f48cdd 29
paulineoonk 8:c4ec359af35d 30 DigitalOut ledred(LED_RED);
paulineoonk 7:05c71a859d27 31 DigitalOut ledblue(LED_BLUE);
paulineoonk 9:285499f48cdd 32 DigitalOut ledgreen(LED_GREEN);
paulineoonk 8:c4ec359af35d 33
paulineoonk 10:518a8617c86e 34 //MVC for calibration
paulineoonk 10:518a8617c86e 35 double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
paulineoonk 10:518a8617c86e 36 //MEAN for calibration - rest
paulineoonk 10:518a8617c86e 37 double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;
paulineoonk 9:285499f48cdd 38
paulineoonk 10:518a8617c86e 39 double emgMEANSUBLB;double emgMEANSUBRB ;double emgMEANSUBLT ;double emgMEANSUBRT ;
paulineoonk 10:518a8617c86e 40 double emgSUMLB;double emgSUMRB;double emgSUMLT;double emgSUMRT;
paulineoonk 10:518a8617c86e 41
paulineoonk 8:c4ec359af35d 42
paulineoonk 7:05c71a859d27 43 bool caldone = false;
paulineoonk 8:c4ec359af35d 44 int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
paulineoonk 8:c4ec359af35d 45
paulineoonk 9:285499f48cdd 46 int Timescalibration = 0;
paulineoonk 10:518a8617c86e 47 int TimescalibrationREST = 0;
paulineoonk 10:518a8617c86e 48
Miriam 0:d5fb29bc0847 49
paulineoonk 9:285499f48cdd 50 // Biquad filters voor Left Bicep (LB)
paulineoonk 9:285499f48cdd 51 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 52 BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 53 BiQuadChain NFLB;
paulineoonk 9:285499f48cdd 54 BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 55 BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 56 BiQuadChain HPFLB;
paulineoonk 9:285499f48cdd 57 BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 58 BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 59 BiQuadChain LPFLB;
paulineoonk 9:285499f48cdd 60
paulineoonk 9:285499f48cdd 61 // Biquad filters voor Right Bicep (RB)
Miriam 0:d5fb29bc0847 62 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 63 BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 64 BiQuadChain NFRB;
paulineoonk 9:285499f48cdd 65 BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 66 BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 67 BiQuadChain HPFRB;
paulineoonk 9:285499f48cdd 68 BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 69 BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 70 BiQuadChain LPFRB;
paulineoonk 9:285499f48cdd 71
paulineoonk 9:285499f48cdd 72 // Biquad filters voor Left Tricep (LT)
paulineoonk 9:285499f48cdd 73 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 74 BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 75 BiQuadChain NFLT;
paulineoonk 9:285499f48cdd 76 BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 77 BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 78 BiQuadChain HPFLT;
paulineoonk 9:285499f48cdd 79 BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 80 BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 81 BiQuadChain LPFLT;
paulineoonk 9:285499f48cdd 82
paulineoonk 9:285499f48cdd 83 // Biquad filters voor Left Tricep (RT)
paulineoonk 9:285499f48cdd 84 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 85 BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 86 BiQuadChain NFRT;
paulineoonk 9:285499f48cdd 87 BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 88 BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 89 BiQuadChain HPFRT;
paulineoonk 9:285499f48cdd 90 BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 91 BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 92 BiQuadChain LPFRT;
paulineoonk 9:285499f48cdd 93
paulineoonk 9:285499f48cdd 94
paulineoonk 7:05c71a859d27 95
paulineoonk 7:05c71a859d27 96 Timer looptime; //moetuiteindelijk weg
paulineoonk 6:e0e5da2c068f 97
paulineoonk 7:05c71a859d27 98 //filters
paulineoonk 8:c4ec359af35d 99 double emgNotchLB;
paulineoonk 8:c4ec359af35d 100 double emgHPLB;
paulineoonk 8:c4ec359af35d 101 double emgAbsHPLB;
paulineoonk 8:c4ec359af35d 102 double emgLPLB;
paulineoonk 8:c4ec359af35d 103
paulineoonk 8:c4ec359af35d 104 double emgNotchRB;
paulineoonk 8:c4ec359af35d 105 double emgHPRB;
paulineoonk 8:c4ec359af35d 106 double emgAbsHPRB;
paulineoonk 8:c4ec359af35d 107 double emgLPRB;
paulineoonk 7:05c71a859d27 108
paulineoonk 8:c4ec359af35d 109 double emgNotchLT;
paulineoonk 8:c4ec359af35d 110 double emgHPLT;
paulineoonk 8:c4ec359af35d 111 double emgAbsHPLT;
paulineoonk 8:c4ec359af35d 112 double emgLPLT;
paulineoonk 7:05c71a859d27 113
paulineoonk 8:c4ec359af35d 114 double emgNotchRT;
paulineoonk 8:c4ec359af35d 115 double emgHPRT;
paulineoonk 8:c4ec359af35d 116 double emgAbsHPRT;
paulineoonk 8:c4ec359af35d 117 double emgLPRT;
Miriam 0:d5fb29bc0847 118
paulineoonk 3:36e706d6b3d2 119 double f = 500; // frequency
paulineoonk 3:36e706d6b3d2 120 double dt = 1/f; // sample frequency
paulineoonk 8:c4ec359af35d 121
paulineoonk 8:c4ec359af35d 122 AnalogIn emgLB(A0); // EMG lezen
paulineoonk 8:c4ec359af35d 123 AnalogIn emgRB(A1);
paulineoonk 8:c4ec359af35d 124 AnalogIn emgLT(A2);
paulineoonk 8:c4ec359af35d 125 AnalogIn emgRT(A3);
Miriam 0:d5fb29bc0847 126
paulineoonk 9:285499f48cdd 127 //float MVCLB = 0.3;
paulineoonk 9:285499f48cdd 128 //float MVCRB = 0.3;
paulineoonk 9:285499f48cdd 129 //float MVCLT = 0.3;
paulineoonk 9:285499f48cdd 130 //float MVCRT = 0.3;
paulineoonk 8:c4ec359af35d 131
paulineoonk 11:b46a4c48c08f 132 // variabelen changePosition
paulineoonk 11:b46a4c48c08f 133 int goalx, goaly;
paulineoonk 11:b46a4c48c08f 134
paulineoonk 8:c4ec359af35d 135 void Filteren()
Miriam 0:d5fb29bc0847 136 {
paulineoonk 6:e0e5da2c068f 137 looptime.reset();
paulineoonk 6:e0e5da2c068f 138 looptime.start();
paulineoonk 8:c4ec359af35d 139
paulineoonk 8:c4ec359af35d 140 //EMG 1
paulineoonk 8:c4ec359af35d 141
paulineoonk 10:518a8617c86e 142 emgNotchLB = NFLB.step(emgLB.read() ); // Notch filter
paulineoonk 10:518a8617c86e 143 emgHPLB = HPFLB.step(emgNotchLB); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 144 emgAbsHPLB = abs(emgHPLB); // Take absolute value
paulineoonk 10:518a8617c86e 145 emgLPLB = LPFLB.step(emgAbsHPLB); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 146 emgMEANSUBLB = emgLPLB - RESTMEANLB; //substract the restmean value
paulineoonk 10:518a8617c86e 147 LBF = emgLPLB/MVCLB; // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
paulineoonk 9:285499f48cdd 148
paulineoonk 10:518a8617c86e 149 emgNotchRB = NFRB.step(emgRB.read()); // Notch filter
paulineoonk 10:518a8617c86e 150 emgHPRB = HPFRB.step(emgNotchRB); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 151 emgAbsHPRB = abs(emgHPRB); // Take absolute value
paulineoonk 10:518a8617c86e 152 emgLPRB = LPFRB.step(emgAbsHPRB); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 153 emgMEANSUBLB = emgLPLB - RESTMEANLB;
paulineoonk 9:285499f48cdd 154 RBF = emgLPRB/MVCRB; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 155
paulineoonk 10:518a8617c86e 156 emgNotchLT = NFLT.step(emgLT.read() ); // Notch filter
paulineoonk 10:518a8617c86e 157 emgHPLT = HPFLT.step(emgNotchLT); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 158 emgAbsHPLT = abs(emgHPLT); // Take absolute value
paulineoonk 10:518a8617c86e 159 emgLPLT = LPFLT.step(emgAbsHPLT); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 160 emgMEANSUBLT = emgLPLT - RESTMEANLT; //substract the restmean value
paulineoonk 9:285499f48cdd 161 LTF = emgLPLT/MVCLT; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 162
paulineoonk 10:518a8617c86e 163 emgNotchRT = NFRT.step(emgRT.read() ); // Notch filter
paulineoonk 10:518a8617c86e 164 emgHPRT = HPFRT.step(emgNotchRT); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 165 emgAbsHPRT = abs(emgHPRT); // Take absolute value
paulineoonk 10:518a8617c86e 166 emgLPRT = LPFRT.step(emgAbsHPRT); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 167 emgMEANSUBRT = emgLPRT - RESTMEANRT; //substract the restmean value
paulineoonk 9:285499f48cdd 168 RTF = emgLPRT/MVCRT; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 169
paulineoonk 8:c4ec359af35d 170 //if (emgFiltered >1)
paulineoonk 8:c4ec359af35d 171 //{
paulineoonk 8:c4ec359af35d 172 // emgFiltered=1.00;
paulineoonk 8:c4ec359af35d 173 //}
paulineoonk 8:c4ec359af35d 174 //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 175 //int maxwaarde = 4096; // = 64x64
paulineoonk 6:e0e5da2c068f 176 //double refP = emgFiltered*maxwaarde;
paulineoonk 6:e0e5da2c068f 177 //return refP; // value between 0 and 4096
paulineoonk 9:285499f48cdd 178
Miriam 0:d5fb29bc0847 179 }
paulineoonk 9:285499f48cdd 180
paulineoonk 10:518a8617c86e 181 void CalibrationEMG()
paulineoonk 7:05c71a859d27 182 {
paulineoonk 7:05c71a859d27 183 Timescalibration++;
paulineoonk 9:285499f48cdd 184
paulineoonk 10:518a8617c86e 185 if(Timescalibration<2000)
paulineoonk 9:285499f48cdd 186 {
paulineoonk 10:518a8617c86e 187
paulineoonk 10:518a8617c86e 188 emgNotchLB = NFLB.step(emgLB.read() );
paulineoonk 10:518a8617c86e 189 emgHPLB = HPFLB.step(emgNotchLB);
paulineoonk 10:518a8617c86e 190 emgAbsHPLB = abs(emgHPLB);
paulineoonk 10:518a8617c86e 191 emgLPLB = LPFLB.step(emgAbsHPLB);
paulineoonk 10:518a8617c86e 192 emgSUMLB += emgLPLB; //SUM all rest values LB
paulineoonk 10:518a8617c86e 193
paulineoonk 11:b46a4c48c08f 194 emgNotchRB = NFRB.step(emgRB.read());
paulineoonk 10:518a8617c86e 195 emgHPRB = HPFRB.step(emgNotchRB);
paulineoonk 10:518a8617c86e 196 emgAbsHPRB = abs(emgHPRB);
paulineoonk 10:518a8617c86e 197 emgLPRB = LPFRB.step(emgAbsHPRB);
paulineoonk 10:518a8617c86e 198 emgSUMRB += emgLPRB; //SUM all rest values RB
paulineoonk 10:518a8617c86e 199
paulineoonk 10:518a8617c86e 200 emgNotchLT = NFLT.step(emgLT.read() );
paulineoonk 10:518a8617c86e 201 emgHPLT = HPFLT.step(emgNotchLT);
paulineoonk 10:518a8617c86e 202 emgAbsHPLT = abs(emgHPLT);
paulineoonk 10:518a8617c86e 203 emgLPLT = LPFLT.step(emgAbsHPLT);
paulineoonk 10:518a8617c86e 204 emgSUMLT += emgLPLT; //SUM all rest values LT
paulineoonk 10:518a8617c86e 205
paulineoonk 10:518a8617c86e 206 emgNotchRT = NFRT.step(emgRT.read() );
paulineoonk 10:518a8617c86e 207 emgHPRT = HPFRT.step(emgNotchRT);
paulineoonk 10:518a8617c86e 208 emgAbsHPRT = abs(emgHPRT);
paulineoonk 10:518a8617c86e 209 emgLPRT = LPFRT.step(emgAbsHPRT);
paulineoonk 10:518a8617c86e 210 emgSUMRT += emgLPRT; //SUM all rest values RT
paulineoonk 10:518a8617c86e 211 }
paulineoonk 10:518a8617c86e 212 if(Timescalibration==1999)
paulineoonk 10:518a8617c86e 213 {
paulineoonk 10:518a8617c86e 214 RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 215 RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 216 RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 217 RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 218 }
paulineoonk 10:518a8617c86e 219 if(Timescalibration>2000 && Timescalibration<6000)
paulineoonk 10:518a8617c86e 220 {
paulineoonk 10:518a8617c86e 221 emgNotchLB = NFLB.step(emgLB.read() );
paulineoonk 10:518a8617c86e 222 emgHPLB = HPFLB.step(emgNotchLB);
paulineoonk 10:518a8617c86e 223 emgAbsHPLB = abs(emgHPLB);
paulineoonk 10:518a8617c86e 224 emgLPLB = LPFLB.step(emgAbsHPLB);
paulineoonk 9:285499f48cdd 225 double emgfinalLB = emgLPLB;
paulineoonk 9:285499f48cdd 226 if (emgfinalLB > MVCLB)
paulineoonk 9:285499f48cdd 227 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 228 MVCLB = emgfinalLB;
paulineoonk 9:285499f48cdd 229 }
paulineoonk 9:285499f48cdd 230 }
paulineoonk 9:285499f48cdd 231
paulineoonk 10:518a8617c86e 232 if(Timescalibration>6000 && Timescalibration<10000)
paulineoonk 9:285499f48cdd 233 {
paulineoonk 10:518a8617c86e 234 emgNotchRB = NFRB.step(emgRB.read());
paulineoonk 10:518a8617c86e 235 emgHPRB = HPFRB.step(emgNotchRB);
paulineoonk 10:518a8617c86e 236 emgAbsHPRB = abs(emgHPRB);
paulineoonk 10:518a8617c86e 237 emgLPRB = LPFRB.step(emgAbsHPRB);
paulineoonk 9:285499f48cdd 238 double emgfinalRB = emgLPRB;
paulineoonk 9:285499f48cdd 239 if (emgfinalRB > MVCRB)
paulineoonk 7:05c71a859d27 240 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 241 MVCRB = emgfinalRB;
paulineoonk 9:285499f48cdd 242 }
paulineoonk 9:285499f48cdd 243 }
paulineoonk 9:285499f48cdd 244
paulineoonk 10:518a8617c86e 245 if(Timescalibration>10000 && Timescalibration<14000)
paulineoonk 9:285499f48cdd 246 {
paulineoonk 10:518a8617c86e 247 emgNotchLT = NFLT.step(emgLT.read() );
paulineoonk 10:518a8617c86e 248 emgHPLT = HPFLT.step(emgNotchLT);
paulineoonk 10:518a8617c86e 249 emgAbsHPLT = abs(emgHPLT);
paulineoonk 10:518a8617c86e 250 emgLPLT = LPFLT.step(emgAbsHPLT);
paulineoonk 9:285499f48cdd 251 double emgfinalLT = emgLPLT;
paulineoonk 9:285499f48cdd 252 if (emgfinalLT > MVCLT)
paulineoonk 9:285499f48cdd 253 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 254 MVCLT = emgfinalLT;
paulineoonk 7:05c71a859d27 255 }
paulineoonk 9:285499f48cdd 256 }
paulineoonk 9:285499f48cdd 257
paulineoonk 10:518a8617c86e 258 if(Timescalibration>14000 && Timescalibration<18000)
paulineoonk 9:285499f48cdd 259 {
paulineoonk 10:518a8617c86e 260 emgNotchRT = NFRT.step(emgRT.read() );
paulineoonk 10:518a8617c86e 261 emgHPRT = HPFRT.step(emgNotchRT);
paulineoonk 10:518a8617c86e 262 emgAbsHPRT = abs(emgHPRT);
paulineoonk 10:518a8617c86e 263 emgLPRT = LPFRT.step(emgAbsHPRT);
paulineoonk 9:285499f48cdd 264 double emgfinalRT = emgLPRT;
paulineoonk 9:285499f48cdd 265 if (emgfinalRT > MVCRT)
paulineoonk 9:285499f48cdd 266 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 267 MVCRT = emgfinalRT;
paulineoonk 9:285499f48cdd 268 }
paulineoonk 9:285499f48cdd 269 }
paulineoonk 9:285499f48cdd 270
paulineoonk 10:518a8617c86e 271 if(Timescalibration>18000)
paulineoonk 7:05c71a859d27 272 {
paulineoonk 7:05c71a859d27 273 caldone=true;
paulineoonk 7:05c71a859d27 274 }
paulineoonk 9:285499f48cdd 275 // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
paulineoonk 9:285499f48cdd 276 //}
paulineoonk 7:05c71a859d27 277 //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
paulineoonk 7:05c71a859d27 278 //return maxi;
paulineoonk 7:05c71a859d27 279 }
paulineoonk 11:b46a4c48c08f 280
paulineoonk 11:b46a4c48c08f 281 double changePosition ()
paulineoonk 11:b46a4c48c08f 282 {
paulineoonk 11:b46a4c48c08f 283 if (RBF>0.3) {
paulineoonk 11:b46a4c48c08f 284 goalx++; // hoe veel verder gaat hij? 1 cm? 10 cm?
paulineoonk 11:b46a4c48c08f 285 }
paulineoonk 11:b46a4c48c08f 286 if (RTF>0.3) {
paulineoonk 11:b46a4c48c08f 287 goalx--;
paulineoonk 11:b46a4c48c08f 288 }
paulineoonk 11:b46a4c48c08f 289 if (LBF>0.3) {
paulineoonk 11:b46a4c48c08f 290 goaly++;
paulineoonk 11:b46a4c48c08f 291 }
paulineoonk 11:b46a4c48c08f 292 if (LTF>0.3) {
paulineoonk 11:b46a4c48c08f 293 goaly--;
paulineoonk 11:b46a4c48c08f 294 }
paulineoonk 11:b46a4c48c08f 295 pc.printf("goalx = %i, goaly = %i \n",goalx, goaly);
paulineoonk 11:b46a4c48c08f 296 }
paulineoonk 7:05c71a859d27 297
paulineoonk 6:e0e5da2c068f 298 /*
paulineoonk 3:36e706d6b3d2 299 double Encoder ()
paulineoonk 3:36e706d6b3d2 300 {
paulineoonk 3:36e706d6b3d2 301 double Huidigepositie = motor1.getPosition ();
paulineoonk 3:36e706d6b3d2 302 return Huidigepositie; // huidige positie = current position
paulineoonk 3:36e706d6b3d2 303 }
paulineoonk 6:e0e5da2c068f 304
paulineoonk 3:36e706d6b3d2 305 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)
Miriam 0:d5fb29bc0847 306 {
paulineoonk 3:36e706d6b3d2 307 double kp = 0.001; // has jet to be scaled
paulineoonk 3:36e706d6b3d2 308 double Proportional= kp*error;
Miriam 0:d5fb29bc0847 309
paulineoonk 3:36e706d6b3d2 310 double kd = 0.0004; // has jet to be scaled
paulineoonk 3:36e706d6b3d2 311 double VelocityError = (error - e_prev)/Ts;
paulineoonk 3:36e706d6b3d2 312 double Derivative = kd*VelocityError;
Miriam 0:d5fb29bc0847 313 e_prev = error;
Miriam 0:d5fb29bc0847 314
paulineoonk 3:36e706d6b3d2 315 double ki = 0.00005; // has jet to be scaled
Miriam 0:d5fb29bc0847 316 e_int = e_int+Ts*error;
paulineoonk 3:36e706d6b3d2 317 double Integrator = ki*e_int;
Miriam 0:d5fb29bc0847 318
Miriam 0:d5fb29bc0847 319
paulineoonk 3:36e706d6b3d2 320 double motorValue = Proportional + Integrator + Derivative;
Miriam 0:d5fb29bc0847 321 return motorValue;
Miriam 0:d5fb29bc0847 322 }
paulineoonk 6:e0e5da2c068f 323
paulineoonk 3:36e706d6b3d2 324 void SetMotor1(double motorValue)
Miriam 0:d5fb29bc0847 325 {
Miriam 0:d5fb29bc0847 326 if (motorValue >= 0)
Miriam 0:d5fb29bc0847 327 {
Miriam 0:d5fb29bc0847 328 M1D = 0;
Miriam 0:d5fb29bc0847 329 }
Miriam 0:d5fb29bc0847 330 else
Miriam 0:d5fb29bc0847 331 {
Miriam 0:d5fb29bc0847 332 M1D = 1;
Miriam 0:d5fb29bc0847 333 }
Miriam 0:d5fb29bc0847 334
Miriam 0:d5fb29bc0847 335 if (fabs(motorValue) > 1)
Miriam 0:d5fb29bc0847 336 {
Miriam 0:d5fb29bc0847 337 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Miriam 0:d5fb29bc0847 338 }
Miriam 0:d5fb29bc0847 339 else
Miriam 0:d5fb29bc0847 340 {
Miriam 0:d5fb29bc0847 341 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Miriam 0:d5fb29bc0847 342 }
Miriam 0:d5fb29bc0847 343 }
paulineoonk 7:05c71a859d27 344 */
paulineoonk 3:36e706d6b3d2 345 void MeasureAndControl ()
Miriam 0:d5fb29bc0847 346 {
paulineoonk 3:36e706d6b3d2 347 // hier the control of the control system
paulineoonk 7:05c71a859d27 348
paulineoonk 9:285499f48cdd 349 if(caldone==false)
paulineoonk 9:285499f48cdd 350 {
paulineoonk 9:285499f48cdd 351 if(button1.read()==false)
paulineoonk 9:285499f48cdd 352 {
paulineoonk 10:518a8617c86e 353 CalibrationEMG();
paulineoonk 9:285499f48cdd 354 }
paulineoonk 9:285499f48cdd 355 }
paulineoonk 9:285499f48cdd 356 if (caldone==true)
paulineoonk 7:05c71a859d27 357
paulineoonk 9:285499f48cdd 358 {
paulineoonk 8:c4ec359af35d 359 Filteren();
paulineoonk 11:b46a4c48c08f 360 changePosition();
paulineoonk 7:05c71a859d27 361 //rest
paulineoonk 9:285499f48cdd 362 }
paulineoonk 7:05c71a859d27 363
paulineoonk 4:5607088ef6f5 364 //double Huidigepositie = Encoder();
paulineoonk 4:5607088ef6f5 365 //double error = (refP - Huidigepositie);// make an error
paulineoonk 4:5607088ef6f5 366 //double motorValue = FeedBackControl(error, e_prev, e_int);
paulineoonk 7:05c71a859d27 367 //double motorValue = refP;
paulineoonk 7:05c71a859d27 368 //SetMotor1(motorValue);
Miriam 0:d5fb29bc0847 369 }
Miriam 0:d5fb29bc0847 370
paulineoonk 8:c4ec359af35d 371 void Tickerfunctie()
paulineoonk 8:c4ec359af35d 372 {
paulineoonk 11:b46a4c48c08f 373 /*pc.printf("emgreadRB = %f , emgFiltered = %f, maxi = %f meanrest = %f\r\n",emgRB.read(), RBF, MVCRB, RESTMEANLB);
paulineoonk 11:b46a4c48c08f 374 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 375 pc.printf("emgreadRT = %f , emgFilteredRT = %f, maxiRT = %f meanrest = %f \r\n",emgRT.read(), RTF, MVCRT,RESTMEANRT);
paulineoonk 10:518a8617c86e 376 pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f meanrest = %f \r\n",emgLT.read(), LTF, MVCLT,RESTMEANLT);
paulineoonk 11:b46a4c48c08f 377 */
paulineoonk 8:c4ec359af35d 378 }
paulineoonk 7:05c71a859d27 379
Miriam 0:d5fb29bc0847 380 int main()
Miriam 0:d5fb29bc0847 381 {
paulineoonk 7:05c71a859d27 382 //voor EMG filteren
paulineoonk 10:518a8617c86e 383 //Left Bicep
paulineoonk 9:285499f48cdd 384 NFLB.add( &N1LB );
paulineoonk 10:518a8617c86e 385 HPFLB.add( &HP1LB ).add( &HP2LB );
paulineoonk 9:285499f48cdd 386 LPFLB.add( &LP1LB ).add( &LP2LB );
paulineoonk 9:285499f48cdd 387
paulineoonk 10:518a8617c86e 388 //Right Bicep
paulineoonk 10:518a8617c86e 389 NFRB.add( &N1RB );
paulineoonk 10:518a8617c86e 390 HPFRB.add( &HP1RB ).add( &HP2RB );
paulineoonk 10:518a8617c86e 391 LPFRB.add( &LP1RB ).add( &LP2RB );
paulineoonk 9:285499f48cdd 392
paulineoonk 10:518a8617c86e 393 //Left Tricep
paulineoonk 10:518a8617c86e 394 NFLT.add( &N1LT );
paulineoonk 10:518a8617c86e 395 HPFLT.add( &HP1LT ).add( &HP2LT );
paulineoonk 10:518a8617c86e 396 LPFLT.add( &LP1LT ).add( &LP2LT );
paulineoonk 10:518a8617c86e 397
paulineoonk 10:518a8617c86e 398 //Right Tricep
paulineoonk 10:518a8617c86e 399 NFRT.add( &N1RT );
paulineoonk 10:518a8617c86e 400 HPFRT.add( &HP1RT ).add( &HP2RT );
paulineoonk 10:518a8617c86e 401 LPFRT.add( &LP1RT ).add( &LP2RT );
paulineoonk 3:36e706d6b3d2 402
paulineoonk 7:05c71a859d27 403 //voor serial
paulineoonk 7:05c71a859d27 404 pc.baud(115200);
paulineoonk 7:05c71a859d27 405
paulineoonk 7:05c71a859d27 406 //motor
paulineoonk 8:c4ec359af35d 407 // M1E.period(PwmPeriod); //set PWMposition at 5000hz
paulineoonk 7:05c71a859d27 408 //Ticker
paulineoonk 7:05c71a859d27 409 Treecko.attach(MeasureAndControl, tijdstap); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
paulineoonk 3:36e706d6b3d2 410 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
paulineoonk 9:285499f48cdd 411 printer.attach(Tickerfunctie,0.4);
paulineoonk 3:36e706d6b3d2 412 while(true)
Miriam 0:d5fb29bc0847 413 {
paulineoonk 3:36e706d6b3d2 414 }
paulineoonk 3:36e706d6b3d2 415
paulineoonk 3:36e706d6b3d2 416
paulineoonk 3:36e706d6b3d2 417 }