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@19:591572f4e4b5, 2017-11-03 (annotated)

Committer:
Gerber
Date:
Fri Nov 03 02:10:11 2017 +0000
Revision:
19:591572f4e4b5
Parent:
18:1e4f697a92cb
Child:
20:14edaecd7413
GEEN FOUTMELDINGEN MET EMG EN ALLES!

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