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@16:2f89d6e25782, 2017-11-02 (annotated)

Committer:
paulineoonk
Date:
Thu Nov 02 13:43:01 2017 +0000
Revision:
16:2f89d6e25782
Parent:
15:1cfe58aea10d
Child:
17:dbdbd1edc260
PID controller toegevoegd, calibratie moet nog. Vergeet minnetje niet bij motor1 --> refp

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