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@15:1cfe58aea10d, 2017-11-02 (annotated)

Committer:
paulineoonk
Date:
Thu Nov 02 09:10:41 2017 +0000
Revision:
15:1cfe58aea10d
Parent:
14:a861ba49107c
Child:
16:2f89d6e25782
State machine met emg werkend, PID moet nog toegevoegd worden;

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