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@32:30a36362f23d, 2017-11-07 (annotated)

Committer:
Gerber
Date:
Tue Nov 07 21:37:13 2017 +0000
Revision:
32:30a36362f23d
Parent:
31:a636a8f590a6
Child:
33:4e5aca9f73e6
KLAAAAAAAAAAR

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
Gerber 28:19cccdd68b5b 8 // Variables
Gerber 27:2d9f172c66ad 9 //State Machine and calibration
Gerber 27:2d9f172c66ad 10 enum States {CalEMG, SelectDevice, EMG, Rest, Demonstration};
paulineoonk 15:1cfe58aea10d 11 States State;
Gerber 18:1e4f697a92cb 12 DigitalIn button (D1);
paulineoonk 15:1cfe58aea10d 13
Gerber 28:19cccdd68b5b 14 //Buttons en leds voor calibration
Gerber 28:19cccdd68b5b 15 DigitalIn button1(PTA4);
Gerber 28:19cccdd68b5b 16 DigitalOut led(D2);
Gerber 28:19cccdd68b5b 17
Gerber 28:19cccdd68b5b 18 //MVC for calibration
Gerber 28:19cccdd68b5b 19 double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
Gerber 28:19cccdd68b5b 20 //MEAN for calibration - rest
Gerber 28:19cccdd68b5b 21 double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;
Gerber 28:19cccdd68b5b 22 double emgMEANSUBLB; double emgMEANSUBRB; double emgMEANSUBLT; double emgMEANSUBRT;
Gerber 28:19cccdd68b5b 23 double emgSUMLB; double emgSUMRB; double emgSUMLT; double emgSUMRT;
Gerber 28:19cccdd68b5b 24
Gerber 28:19cccdd68b5b 25 bool caldone = false;
Gerber 28:19cccdd68b5b 26 int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
Gerber 28:19cccdd68b5b 27
Gerber 28:19cccdd68b5b 28 int Timescalibration = 0;
Gerber 28:19cccdd68b5b 29 int TimescalibrationREST = 0;
Gerber 28:19cccdd68b5b 30
Gerber 28:19cccdd68b5b 31 //Encoder and motor
paulineoonk 15:1cfe58aea10d 32 double Huidigepositie1;
paulineoonk 15:1cfe58aea10d 33 double Huidigepositie2;
paulineoonk 15:1cfe58aea10d 34 double motorValue1;
paulineoonk 15:1cfe58aea10d 35 double motorValue2;
Gerber 27:2d9f172c66ad 36 Ticker Treecko; //We make an awesome ticker for our control system
paulineoonk 16:2f89d6e25782 37 PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed
paulineoonk 16:2f89d6e25782 38 PwmOut M2E(D5);
paulineoonk 16:2f89d6e25782 39 DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control
paulineoonk 16:2f89d6e25782 40 Encoder motor1(D13,D12,true);
paulineoonk 16:2f89d6e25782 41 Encoder motor2(D9,D8,true);
paulineoonk 16:2f89d6e25782 42 DigitalOut M2D(D4);
Gerber 28:19cccdd68b5b 43 double PwmPeriod = 1.0/5000.0;
Miriam 0:d5fb29bc0847 44
Gerber 27:2d9f172c66ad 45 //Demonstration
paulineoonk 16:2f89d6e25782 46 AnalogIn potMeter2(A1);
paulineoonk 16:2f89d6e25782 47 AnalogIn potMeter1(A2);
paulineoonk 16:2f89d6e25782 48
paulineoonk 3:36e706d6b3d2 49 MODSERIAL pc(USBTX,USBRX);
paulineoonk 3:36e706d6b3d2 50
Gerber 28:19cccdd68b5b 51 //PID
Gerber 32:30a36362f23d 52 const double Ts = 0.002f; // tickertime/ sample time
paulineoonk 16:2f89d6e25782 53 double e_prev = 0;
paulineoonk 16:2f89d6e25782 54 double e_int = 0;
paulineoonk 16:2f89d6e25782 55 double e_prev2 = 0;
paulineoonk 16:2f89d6e25782 56 double e_int2 = 0;
paulineoonk 16:2f89d6e25782 57
Gerber 28:19cccdd68b5b 58 // EMG and Filters
Gerber 27:2d9f172c66ad 59 // Biquad filters voor Left Biceps (LB): Notch, High-pass and Low-pass filter
paulineoonk 9:285499f48cdd 60 BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 61 BiQuadChain NFLB;
paulineoonk 9:285499f48cdd 62 BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 63 BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 64 BiQuadChain HPFLB;
paulineoonk 9:285499f48cdd 65 BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 66 BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 67 BiQuadChain LPFLB;
paulineoonk 9:285499f48cdd 68
Gerber 27:2d9f172c66ad 69 // Biquad filters voor Right Biceps (RB): Notch, High-pass and Low-pass filter
paulineoonk 9:285499f48cdd 70 BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 71 BiQuadChain NFRB;
paulineoonk 9:285499f48cdd 72 BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 73 BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 74 BiQuadChain HPFRB;
paulineoonk 9:285499f48cdd 75 BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 76 BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 77 BiQuadChain LPFRB;
paulineoonk 9:285499f48cdd 78
Gerber 27:2d9f172c66ad 79 // Biquad filters voor Left Triceps (LT): Notch, High-pass and Low-pass filter
paulineoonk 9:285499f48cdd 80 BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 81 BiQuadChain NFLT;
paulineoonk 9:285499f48cdd 82 BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 83 BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 84 BiQuadChain HPFLT;
paulineoonk 9:285499f48cdd 85 BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 86 BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 87 BiQuadChain LPFLT;
paulineoonk 9:285499f48cdd 88
Gerber 27:2d9f172c66ad 89 // Biquad filters for Right Triceps (RT): Notch, High-pass and Low-pass filter
paulineoonk 9:285499f48cdd 90 BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 91 BiQuadChain NFRT;
paulineoonk 9:285499f48cdd 92 BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 93 BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 94 BiQuadChain HPFRT;
paulineoonk 9:285499f48cdd 95 BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 96 BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 97 BiQuadChain LPFRT;
paulineoonk 9:285499f48cdd 98
paulineoonk 8:c4ec359af35d 99 double emgNotchLB;
paulineoonk 8:c4ec359af35d 100 double emgHPLB;
paulineoonk 8:c4ec359af35d 101 double emgAbsHPLB;
paulineoonk 8:c4ec359af35d 102 double emgLPLB;
paulineoonk 8:c4ec359af35d 103
paulineoonk 8:c4ec359af35d 104 double emgNotchRB;
paulineoonk 8:c4ec359af35d 105 double emgHPRB;
paulineoonk 8:c4ec359af35d 106 double emgAbsHPRB;
paulineoonk 8:c4ec359af35d 107 double emgLPRB;
paulineoonk 7:05c71a859d27 108
paulineoonk 8:c4ec359af35d 109 double emgNotchLT;
paulineoonk 8:c4ec359af35d 110 double emgHPLT;
paulineoonk 8:c4ec359af35d 111 double emgAbsHPLT;
paulineoonk 8:c4ec359af35d 112 double emgLPLT;
paulineoonk 7:05c71a859d27 113
paulineoonk 8:c4ec359af35d 114 double emgNotchRT;
paulineoonk 8:c4ec359af35d 115 double emgHPRT;
paulineoonk 8:c4ec359af35d 116 double emgAbsHPRT;
paulineoonk 8:c4ec359af35d 117 double emgLPRT;
Miriam 0:d5fb29bc0847 118
Gerber 28:19cccdd68b5b 119 AnalogIn emgLB(A0); // read EMG
paulineoonk 8:c4ec359af35d 120 AnalogIn emgRB(A1);
paulineoonk 8:c4ec359af35d 121 AnalogIn emgLT(A2);
paulineoonk 8:c4ec359af35d 122 AnalogIn emgRT(A3);
Miriam 0:d5fb29bc0847 123
Gerber 28:19cccdd68b5b 124 volatile double LBF;
Gerber 28:19cccdd68b5b 125 volatile double RBF;
Gerber 28:19cccdd68b5b 126 volatile double LTF;
Gerber 28:19cccdd68b5b 127 volatile double RTF;
Gerber 28:19cccdd68b5b 128
Gerber 28:19cccdd68b5b 129 // RKI
Gerber 18:1e4f697a92cb 130 double pi = 3.14159265359;
Gerber 28:19cccdd68b5b 131 double q1 = (pi/2); //Reference position angle 1 in radiance
Gerber 28:19cccdd68b5b 132 double q2 = -(pi/2); //Reference position angle 2 in radiance
Gerber 28:19cccdd68b5b 133 const double L1 = 0.30; //Length arm 1 in mm
Gerber 28:19cccdd68b5b 134 const double L2 = 0.38; //Length arm 2 in mm
Gerber 28:19cccdd68b5b 135 double B1 = 1; //Friction constant motor 1
Gerber 28:19cccdd68b5b 136 double B2 = 1; //Friction constant motor 2
Gerber 28:19cccdd68b5b 137 double K = 1; //Spring constant movement from end-effector position to setpoint position
Gerber 28:19cccdd68b5b 138 double Tijd = 1; //Timestep value
Gerber 28:19cccdd68b5b 139 double Rsx = 0.38; //Reference x-component of the setpoint radius
Gerber 28:19cccdd68b5b 140 double Rsy = 0.30; //Reference y-component of the setpoint radius
Gerber 28:19cccdd68b5b 141 double refP = 0; //Reference position motor 1
Gerber 28:19cccdd68b5b 142 double refP2 = 0; //Reference position motor 2
Gerber 18:1e4f697a92cb 143 double Rex = cos(q1)*L1 - sin(q2)*L2; //The x-component of the end-effector radius
Gerber 18:1e4f697a92cb 144 double Rey = sin(q1)*L1 + cos(q2)*L2; //The y-component of the end-effector radius
Gerber 18:1e4f697a92cb 145 double R1x = 0; //The x-component of the joint 1 radius
Gerber 18:1e4f697a92cb 146 double R1y = 0; //The y-component of the joint 1 radius
Gerber 18:1e4f697a92cb 147 double R2x = cos(q1)*L1; //The x-component of the joint 2 radius
Gerber 27:2d9f172c66ad 148 double R2y = sin(q1)*L1; //The y-component of the joint 2 radius
Gerber 18:1e4f697a92cb 149 double Fx = 0;
Gerber 18:1e4f697a92cb 150 double Fy = 0;
Gerber 18:1e4f697a92cb 151 double Tor1 = 0;
Gerber 18:1e4f697a92cb 152 double Tor2 = 0;
Gerber 18:1e4f697a92cb 153 double w1= 0;
Gerber 18:1e4f697a92cb 154 double w2= 0;
Gerber 18:1e4f697a92cb 155
Gerber 28:19cccdd68b5b 156 // Functions
paulineoonk 8:c4ec359af35d 157 void Filteren()
Gerber 27:2d9f172c66ad 158 {
Gerber 28:19cccdd68b5b 159 emgNotchLB = NFLB.step(emgLB.read() ); // Notch filter
paulineoonk 10:518a8617c86e 160 emgHPLB = HPFLB.step(emgNotchLB); // High-pass filter: also normalises around 0.
Gerber 28:19cccdd68b5b 161 emgAbsHPLB = abs(emgHPLB); // Take absolute value
Gerber 28:19cccdd68b5b 162 emgLPLB = LPFLB.step(emgAbsHPLB); // Low-pass filter: creates envelope
Gerber 28:19cccdd68b5b 163 emgMEANSUBLB = emgLPLB - RESTMEANLB; // Substract the restmean value
Gerber 28:19cccdd68b5b 164 LBF = emgLPLB/MVCLB; // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
paulineoonk 9:285499f48cdd 165
Gerber 28:19cccdd68b5b 166 emgNotchRB = NFRB.step(emgRB.read());
Gerber 28:19cccdd68b5b 167 emgHPRB = HPFRB.step(emgNotchRB);
Gerber 28:19cccdd68b5b 168 emgAbsHPRB = abs(emgHPRB);
Gerber 28:19cccdd68b5b 169 emgLPRB = LPFRB.step(emgAbsHPRB);
paulineoonk 10:518a8617c86e 170 emgMEANSUBLB = emgLPLB - RESTMEANLB;
Gerber 28:19cccdd68b5b 171 RBF = emgLPRB/MVCRB;
paulineoonk 8:c4ec359af35d 172
Gerber 28:19cccdd68b5b 173 emgNotchLT = NFLT.step(emgLT.read() );
Gerber 28:19cccdd68b5b 174 emgHPLT = HPFLT.step(emgNotchLT);
Gerber 28:19cccdd68b5b 175 emgAbsHPLT = abs(emgHPLT);
Gerber 28:19cccdd68b5b 176 emgLPLT = LPFLT.step(emgAbsHPLT);
Gerber 28:19cccdd68b5b 177 emgMEANSUBLT = emgLPLT - RESTMEANLT;
Gerber 28:19cccdd68b5b 178 LTF = emgLPLT/MVCLT;
paulineoonk 8:c4ec359af35d 179
Gerber 28:19cccdd68b5b 180 emgNotchRT = NFRT.step(emgRT.read() );
Gerber 28:19cccdd68b5b 181 emgHPRT = HPFRT.step(emgNotchRT);
Gerber 28:19cccdd68b5b 182 emgAbsHPRT = abs(emgHPRT);
Gerber 28:19cccdd68b5b 183 emgLPRT = LPFRT.step(emgAbsHPRT);
Gerber 28:19cccdd68b5b 184 emgMEANSUBRT = emgLPRT - RESTMEANRT;
Gerber 28:19cccdd68b5b 185 RTF = emgLPRT/MVCRT;
Miriam 0:d5fb29bc0847 186 }
paulineoonk 9:285499f48cdd 187
paulineoonk 10:518a8617c86e 188 void CalibrationEMG()
paulineoonk 7:05c71a859d27 189 {
Gerber 20:14edaecd7413 190 pc.printf("Timescalibration = %i \r\n",Timescalibration);
paulineoonk 7:05c71a859d27 191 Timescalibration++;
paulineoonk 9:285499f48cdd 192
paulineoonk 10:518a8617c86e 193 if(Timescalibration<2000)
paulineoonk 9:285499f48cdd 194 {
Gerber 30:b76c27e4730c 195 pc.printf("calibration rest EMG, 12 seconds \r\n");
charloverwijk 17:dbdbd1edc260 196 led = 1;
paulineoonk 10:518a8617c86e 197
Gerber 27:2d9f172c66ad 198 emgNotchLB = NFLB.step(emgLB.read() );
Gerber 27:2d9f172c66ad 199 emgHPLB = HPFLB.step(emgNotchLB);
Gerber 27:2d9f172c66ad 200 emgAbsHPLB = abs(emgHPLB);
Gerber 27:2d9f172c66ad 201 emgLPLB = LPFLB.step(emgAbsHPLB);
Gerber 27:2d9f172c66ad 202 emgSUMLB += emgLPLB; //SUM all rest values LB
paulineoonk 10:518a8617c86e 203
Gerber 27:2d9f172c66ad 204 emgNotchRB = NFRB.step(emgRB.read());
Gerber 27:2d9f172c66ad 205 emgHPRB = HPFRB.step(emgNotchRB);
Gerber 27:2d9f172c66ad 206 emgAbsHPRB = abs(emgHPRB);
Gerber 27:2d9f172c66ad 207 emgLPRB = LPFRB.step(emgAbsHPRB);
Gerber 27:2d9f172c66ad 208 emgSUMRB += emgLPRB; //SUM all rest values RB
paulineoonk 10:518a8617c86e 209
Gerber 27:2d9f172c66ad 210 emgNotchLT = NFLT.step(emgLT.read() );
Gerber 27:2d9f172c66ad 211 emgHPLT = HPFLT.step(emgNotchLT);
Gerber 27:2d9f172c66ad 212 emgAbsHPLT = abs(emgHPLT);
Gerber 27:2d9f172c66ad 213 emgLPLT = LPFLT.step(emgAbsHPLT);
Gerber 27:2d9f172c66ad 214 emgSUMLT += emgLPLT; //SUM all rest values LT
paulineoonk 10:518a8617c86e 215
Gerber 27:2d9f172c66ad 216 emgNotchRT = NFRT.step(emgRT.read() );
Gerber 27:2d9f172c66ad 217 emgHPRT = HPFRT.step(emgNotchRT);
Gerber 27:2d9f172c66ad 218 emgAbsHPRT = abs(emgHPRT);
Gerber 27:2d9f172c66ad 219 emgLPRT = LPFRT.step(emgAbsHPRT);
Gerber 27:2d9f172c66ad 220 emgSUMRT += emgLPRT; //SUM all rest values RT
paulineoonk 10:518a8617c86e 221 }
paulineoonk 10:518a8617c86e 222 if(Timescalibration==1999)
paulineoonk 10:518a8617c86e 223 {
charloverwijk 17:dbdbd1edc260 224 led = 0;
paulineoonk 10:518a8617c86e 225 RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 226 RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 227 RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 228 RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 229 }
Gerber 26:bfb1ae203c11 230 if(Timescalibration>2000 && Timescalibration<3000)
paulineoonk 10:518a8617c86e 231 {
Gerber 30:b76c27e4730c 232 pc.printf("maximum left biceps, 6 seconds \r\n");
charloverwijk 17:dbdbd1edc260 233 led = 1;
Gerber 27:2d9f172c66ad 234 emgNotchLB = NFLB.step(emgLB.read() );
Gerber 27:2d9f172c66ad 235 emgHPLB = HPFLB.step(emgNotchLB);
Gerber 27:2d9f172c66ad 236 emgAbsHPLB = abs(emgHPLB);
Gerber 27:2d9f172c66ad 237 emgLPLB = LPFLB.step(emgAbsHPLB);
Gerber 27:2d9f172c66ad 238 double emgfinalLB = emgLPLB;
Gerber 27:2d9f172c66ad 239 if (emgfinalLB > MVCLB)
Gerber 27:2d9f172c66ad 240 { //determine what the highest reachable emg signal is
Gerber 27:2d9f172c66ad 241 MVCLB = emgfinalLB;
Gerber 27:2d9f172c66ad 242 }
paulineoonk 9:285499f48cdd 243 }
paulineoonk 9:285499f48cdd 244
Gerber 26:bfb1ae203c11 245 if(Timescalibration>3000 && Timescalibration<4000)
paulineoonk 9:285499f48cdd 246 {
Gerber 30:b76c27e4730c 247 pc.printf(" maximum right biceps, 6 seconds \r\n");
charloverwijk 17:dbdbd1edc260 248 led = 0;
Gerber 27:2d9f172c66ad 249 emgNotchRB = NFRB.step(emgRB.read());
Gerber 27:2d9f172c66ad 250 emgHPRB = HPFRB.step(emgNotchRB);
Gerber 27:2d9f172c66ad 251 emgAbsHPRB = abs(emgHPRB);
Gerber 27:2d9f172c66ad 252 emgLPRB = LPFRB.step(emgAbsHPRB);
Gerber 27:2d9f172c66ad 253 double emgfinalRB = emgLPRB;
Gerber 27:2d9f172c66ad 254 if (emgfinalRB > MVCRB)
Gerber 27:2d9f172c66ad 255 { //determine what the highest reachable emg signal is
Gerber 27:2d9f172c66ad 256 MVCRB = emgfinalRB;
Gerber 27:2d9f172c66ad 257 }
paulineoonk 9:285499f48cdd 258 }
paulineoonk 9:285499f48cdd 259
Gerber 26:bfb1ae203c11 260 if(Timescalibration>4000 && Timescalibration<5000)
paulineoonk 9:285499f48cdd 261 {
Gerber 30:b76c27e4730c 262 pc.printf("maximum left triceps, 6 seconds \r\n");
charloverwijk 17:dbdbd1edc260 263 led = 1;
Gerber 27:2d9f172c66ad 264 emgNotchLT = NFLT.step(emgLT.read() );
Gerber 27:2d9f172c66ad 265 emgHPLT = HPFLT.step(emgNotchLT);
Gerber 27:2d9f172c66ad 266 emgAbsHPLT = abs(emgHPLT);
Gerber 27:2d9f172c66ad 267 emgLPLT = LPFLT.step(emgAbsHPLT);
Gerber 27:2d9f172c66ad 268 double emgfinalLT = emgLPLT;
Gerber 27:2d9f172c66ad 269 if (emgfinalLT > MVCLT)
Gerber 27:2d9f172c66ad 270 { //determine what the highest reachable emg signal is
Gerber 27:2d9f172c66ad 271 MVCLT = emgfinalLT;
Gerber 27:2d9f172c66ad 272 }
paulineoonk 9:285499f48cdd 273 }
paulineoonk 9:285499f48cdd 274
Gerber 26:bfb1ae203c11 275 if(Timescalibration>5000 && Timescalibration<6000)
paulineoonk 9:285499f48cdd 276 {
Gerber 30:b76c27e4730c 277 pc.printf("maximum right triceps, 6 seconds \r\n");
Gerber 27:2d9f172c66ad 278 emgNotchRT = NFRT.step(emgRT.read() );
Gerber 27:2d9f172c66ad 279 emgHPRT = HPFRT.step(emgNotchRT);
Gerber 27:2d9f172c66ad 280 emgAbsHPRT = abs(emgHPRT);
Gerber 27:2d9f172c66ad 281 emgLPRT = LPFRT.step(emgAbsHPRT);
Gerber 27:2d9f172c66ad 282 double emgfinalRT = emgLPRT;
Gerber 27:2d9f172c66ad 283 if (emgfinalRT > MVCRT)
Gerber 27:2d9f172c66ad 284 { //determine what the highest reachable emg signal is
Gerber 27:2d9f172c66ad 285 MVCRT = emgfinalRT;
Gerber 27:2d9f172c66ad 286 }
paulineoonk 9:285499f48cdd 287 }
paulineoonk 9:285499f48cdd 288
Gerber 26:bfb1ae203c11 289 if(Timescalibration>6000)
paulineoonk 7:05c71a859d27 290 {
Gerber 28:19cccdd68b5b 291 pc.printf("calibration finished");
Gerber 20:14edaecd7413 292 State = SelectDevice;
paulineoonk 7:05c71a859d27 293 }
paulineoonk 7:05c71a859d27 294 }
paulineoonk 16:2f89d6e25782 295
Gerber 18:1e4f697a92cb 296 void RKI()
Gerber 18:1e4f697a92cb 297 {
Gerber 18:1e4f697a92cb 298 Rex = cos(q1)*L1 - sin(q2)*L2;
Gerber 18:1e4f697a92cb 299 Rey = sin(q1)*L1 + cos(q2)*L2;
Gerber 18:1e4f697a92cb 300 R2x = cos(q1)*L1;
Gerber 18:1e4f697a92cb 301 R2y = sin(q1)*L1;
Gerber 18:1e4f697a92cb 302 Fx = (Rsx-Rex)*K;
Gerber 18:1e4f697a92cb 303 Fy = (Rsy-Rey)*K;
Gerber 18:1e4f697a92cb 304 Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx;
Gerber 18:1e4f697a92cb 305 Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx;
Gerber 18:1e4f697a92cb 306 w1 = Tor1/B1;
Gerber 18:1e4f697a92cb 307 w2 = Tor2/B2;
Gerber 18:1e4f697a92cb 308 q1 = q1 + w1*Tijd;
Gerber 18:1e4f697a92cb 309 q2 = q2 + w2*Tijd;
Gerber 18:1e4f697a92cb 310
Gerber 31:a636a8f590a6 311 refP = (((0.5*pi) - q1)/(2*pi));
Gerber 31:a636a8f590a6 312 refP2 = (( q1 + q2)/(2*pi)); //Get reference positions
Gerber 18:1e4f697a92cb 313 }
Gerber 18:1e4f697a92cb 314
Gerber 32:30a36362f23d 315 void SetpointRobot() // Get position from Potmeters
Gerber 18:1e4f697a92cb 316 {
Gerber 18:1e4f697a92cb 317 double Potmeterwaarde2 = potMeter2.read();
Gerber 18:1e4f697a92cb 318 double Potmeterwaarde1 = potMeter1.read();
Gerber 18:1e4f697a92cb 319
Gerber 18:1e4f697a92cb 320 if (Potmeterwaarde2>0.6) {
Gerber 28:19cccdd68b5b 321 Rsx += 0.001; //increases 1 mm when potmetervalue above 0.6
Gerber 18:1e4f697a92cb 322 }
Gerber 18:1e4f697a92cb 323 else if (Potmeterwaarde2<0.4) {
Gerber 28:19cccdd68b5b 324 Rsx -= 0.001; //decreases 1 mm when potmetervalue below 0.4
Gerber 18:1e4f697a92cb 325 }
Gerber 28:19cccdd68b5b 326 else { //x value of setpoint doesn't change
Gerber 27:2d9f172c66ad 327 }
Gerber 18:1e4f697a92cb 328
Gerber 28:19cccdd68b5b 329 if (Potmeterwaarde1>0.6) { //increases 1 mm when potmetervalue above 0.6
Gerber 18:1e4f697a92cb 330 Rsy += 0.001;
Gerber 18:1e4f697a92cb 331 }
Gerber 28:19cccdd68b5b 332 else if (Potmeterwaarde1<0.4) { //decreases 1 mm when potmetervalue below 0.4
Gerber 18:1e4f697a92cb 333 Rsy -= 0.001;
Gerber 18:1e4f697a92cb 334 }
Gerber 28:19cccdd68b5b 335 else { //y value of setpoint doesn't change
Gerber 27:2d9f172c66ad 336 }
Gerber 18:1e4f697a92cb 337 }
Gerber 29:69cc48b3feaa 338
Gerber 32:30a36362f23d 339 void changePosition () // Get position from EMG signals
Gerber 29:69cc48b3feaa 340 {
Gerber 29:69cc48b3feaa 341 if (RBF>0.5) {
Gerber 32:30a36362f23d 342 Rsx +=0.001; // increases 1 mm
Gerber 29:69cc48b3feaa 343 }
Gerber 29:69cc48b3feaa 344 else {}
Gerber 29:69cc48b3feaa 345 if (RTF>0.5) {
Gerber 29:69cc48b3feaa 346 Rsx -=0.001;
Gerber 29:69cc48b3feaa 347 }
Gerber 29:69cc48b3feaa 348 else {}
Gerber 29:69cc48b3feaa 349 if (LBF>0.5) {
Gerber 29:69cc48b3feaa 350 Rsy +=0.001;
Gerber 29:69cc48b3feaa 351 }
Gerber 29:69cc48b3feaa 352 else {}
Gerber 29:69cc48b3feaa 353 if (LTF>0.5) {
Gerber 29:69cc48b3feaa 354 Rsy -=0.001;
Gerber 29:69cc48b3feaa 355 }
Gerber 29:69cc48b3feaa 356 else {}
Gerber 29:69cc48b3feaa 357 }
Gerber 29:69cc48b3feaa 358
Gerber 32:30a36362f23d 359 double FeedBackControl(double error, double &e_prev, double &e_int) // PID controller motor 1
paulineoonk 16:2f89d6e25782 360 {
Gerber 31:a636a8f590a6 361 double kp = 0.0008*4200; // kind of scaled.
Gerber 32:30a36362f23d 362 double Proportional= kp*error;
paulineoonk 16:2f89d6e25782 363
Gerber 31:a636a8f590a6 364 double kd = 0.0008*4200; // kind of scaled.
Gerber 19:591572f4e4b5 365 double VelocityError = (error - e_prev)/Ts;
Gerber 19:591572f4e4b5 366 double Derivative = kd*VelocityError;
paulineoonk 16:2f89d6e25782 367 e_prev = error;
paulineoonk 16:2f89d6e25782 368
Gerber 31:a636a8f590a6 369 double ki = 0.001*4200; // kind of scaled.
paulineoonk 16:2f89d6e25782 370 e_int = e_int+Ts*error;
Gerber 19:591572f4e4b5 371 double Integrator = ki*e_int;
paulineoonk 16:2f89d6e25782 372
Gerber 31:a636a8f590a6 373 double motorValue = (Proportional + Integrator + Derivative)/4200;
paulineoonk 16:2f89d6e25782 374 return motorValue;
paulineoonk 16:2f89d6e25782 375 }
paulineoonk 16:2f89d6e25782 376
Gerber 32:30a36362f23d 377 double FeedBackControl2(double error2, double &e_prev2, double &e_int2) // PID controller motor 2
paulineoonk 16:2f89d6e25782 378 {
Gerber 31:a636a8f590a6 379 double kp2 = 0.0008*4200; // kind of scaled.
Gerber 19:591572f4e4b5 380 double Proportional2= kp2*error2;
paulineoonk 16:2f89d6e25782 381
Gerber 31:a636a8f590a6 382 double kd2 = 0.0008*4200; // kind of scaled.
Gerber 19:591572f4e4b5 383 double VelocityError2 = (error2 - e_prev2)/Ts;
Gerber 19:591572f4e4b5 384 double Derivative2 = kd2*VelocityError2;
paulineoonk 16:2f89d6e25782 385 e_prev2 = error2;
paulineoonk 16:2f89d6e25782 386
Gerber 31:a636a8f590a6 387 double ki2 = 0.00005*4200; // kind of scaled.
paulineoonk 16:2f89d6e25782 388 e_int2 = e_int2+Ts*error2;
Gerber 19:591572f4e4b5 389 double Integrator2 = ki2*e_int2;
Gerber 18:1e4f697a92cb 390
Gerber 19:591572f4e4b5 391 double motorValue2 = Proportional2 + Integrator2 + Derivative2;
paulineoonk 16:2f89d6e25782 392 return motorValue2;
paulineoonk 16:2f89d6e25782 393 }
paulineoonk 16:2f89d6e25782 394
paulineoonk 16:2f89d6e25782 395 void SetMotor1(double motorValue)
paulineoonk 16:2f89d6e25782 396 {
paulineoonk 16:2f89d6e25782 397 if (motorValue >= 0) {
paulineoonk 16:2f89d6e25782 398 M1D = 0;
paulineoonk 16:2f89d6e25782 399 }
paulineoonk 16:2f89d6e25782 400 else {
paulineoonk 16:2f89d6e25782 401 M1D = 1;
paulineoonk 16:2f89d6e25782 402 }
paulineoonk 16:2f89d6e25782 403 if (fabs(motorValue) > 1) {
Gerber 28:19cccdd68b5b 404 M1E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
paulineoonk 16:2f89d6e25782 405 }
paulineoonk 16:2f89d6e25782 406 else {
Gerber 28:19cccdd68b5b 407 M1E = fabs(motorValue); //absolute velocity determined, motor is "off" at value of 0
paulineoonk 16:2f89d6e25782 408 }
paulineoonk 16:2f89d6e25782 409 }
paulineoonk 16:2f89d6e25782 410
paulineoonk 16:2f89d6e25782 411 void SetMotor2(double motorValue2)
paulineoonk 16:2f89d6e25782 412 {
paulineoonk 16:2f89d6e25782 413 if (motorValue2 >= 0) {
Gerber 18:1e4f697a92cb 414 M2D = 1;
paulineoonk 16:2f89d6e25782 415 }
paulineoonk 16:2f89d6e25782 416 else {
Gerber 18:1e4f697a92cb 417 M2D = 0;
paulineoonk 16:2f89d6e25782 418 }
paulineoonk 16:2f89d6e25782 419 if (fabs(motorValue2) > 1) {
Gerber 28:19cccdd68b5b 420 M2E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
paulineoonk 16:2f89d6e25782 421 }
paulineoonk 16:2f89d6e25782 422 else {
Gerber 28:19cccdd68b5b 423 M2E = fabs(motorValue2); //absolute velocity determined, motor is "off" at value of 0
paulineoonk 16:2f89d6e25782 424 }
paulineoonk 16:2f89d6e25782 425 }
paulineoonk 16:2f89d6e25782 426
paulineoonk 16:2f89d6e25782 427 void MeasureAndControl(void)
paulineoonk 16:2f89d6e25782 428 {
Gerber 18:1e4f697a92cb 429 RKI();
Gerber 27:2d9f172c66ad 430 // control of 1st motor
Gerber 31:a636a8f590a6 431 double Huidigepositie = motor1.getPosition()/4200;
paulineoonk 16:2f89d6e25782 432 double error = (refP - Huidigepositie);// make an error
paulineoonk 16:2f89d6e25782 433 double motorValue = FeedBackControl(error, e_prev, e_int);
paulineoonk 16:2f89d6e25782 434 SetMotor1(motorValue);
Gerber 27:2d9f172c66ad 435 // control of 2nd motor
Gerber 31:a636a8f590a6 436 double Huidigepositie2 = motor2.getPosition()/4200;
paulineoonk 16:2f89d6e25782 437 double error2 = (refP2 - Huidigepositie2);// make an error
paulineoonk 16:2f89d6e25782 438 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
paulineoonk 16:2f89d6e25782 439 SetMotor2(motorValue2);
paulineoonk 16:2f89d6e25782 440 }
paulineoonk 7:05c71a859d27 441
paulineoonk 14:a861ba49107c 442 void Loop_funtion()
paulineoonk 14:a861ba49107c 443 {
Gerber 20:14edaecd7413 444 pc.printf("state machine begint \r\n");
Gerber 27:2d9f172c66ad 445 switch(State)
Gerber 27:2d9f172c66ad 446 {
paulineoonk 14:a861ba49107c 447 case CalEMG: // Calibration EMG
paulineoonk 15:1cfe58aea10d 448 CalibrationEMG(); //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
paulineoonk 14:a861ba49107c 449 break;
Gerber 32:30a36362f23d 450 case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
Gerber 24:847321a23e60 451 State = EMG;
Gerber 27:2d9f172c66ad 452 if (button==1) {
paulineoonk 15:1cfe58aea10d 453 State=EMG;
paulineoonk 14:a861ba49107c 454 }
Gerber 32:30a36362f23d 455 else { // button==0
Gerber 26:bfb1ae203c11 456 State=Demonstration;
Gerber 27:2d9f172c66ad 457 }
paulineoonk 14:a861ba49107c 458 break;
Gerber 27:2d9f172c66ad 459 case EMG: //Control by EMG
paulineoonk 14:a861ba49107c 460 Filteren();
paulineoonk 14:a861ba49107c 461 changePosition();
paulineoonk 16:2f89d6e25782 462 MeasureAndControl();
Gerber 31:a636a8f590a6 463 if (button==0) {
Gerber 31:a636a8f590a6 464 State=Rest;
Gerber 31:a636a8f590a6 465 }
Gerber 31:a636a8f590a6 466 else {}
paulineoonk 14:a861ba49107c 467 break;
Gerber 27:2d9f172c66ad 468 case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions.
Gerber 32:30a36362f23d 469 refP=refP;
Gerber 32:30a36362f23d 470 refP2=refP2;
Gerber 32:30a36362f23d 471 double Huidigepositie = motor1.getPosition()/4200;
Gerber 32:30a36362f23d 472 double error = (refP - Huidigepositie);// make an error
Gerber 32:30a36362f23d 473 double motorValue = FeedBackControl(error, e_prev, e_int);
Gerber 32:30a36362f23d 474 SetMotor1(motorValue);
Gerber 32:30a36362f23d 475 // control of 2nd motor
Gerber 32:30a36362f23d 476 double Huidigepositie2 = motor2.getPosition()/4200;
Gerber 32:30a36362f23d 477 double error2 = (refP2 - Huidigepositie2);// make an error
Gerber 32:30a36362f23d 478 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Gerber 32:30a36362f23d 479 SetMotor2(motorValue2);
Gerber 32:30a36362f23d 480
Gerber 31:a636a8f590a6 481 if ( button==1) {
Gerber 31:a636a8f590a6 482 State=EMG;
Gerber 31:a636a8f590a6 483 }
Gerber 31:a636a8f590a6 484 else {}
Gerber 31:a636a8f590a6 485 break;
Gerber 32:30a36362f23d 486 case Demonstration: // Control with Potmeters
Gerber 28:19cccdd68b5b 487 SetpointRobot();
Gerber 28:19cccdd68b5b 488 MeasureAndControl();
paulineoonk 14:a861ba49107c 489 break;
paulineoonk 15:1cfe58aea10d 490 }
paulineoonk 14:a861ba49107c 491 }
Miriam 0:d5fb29bc0847 492
Gerber 31:a636a8f590a6 493 int main()
Miriam 0:d5fb29bc0847 494 {
Gerber 32:30a36362f23d 495 //for filtering EMG
Gerber 31:a636a8f590a6 496 //Left Biceps
paulineoonk 9:285499f48cdd 497 NFLB.add( &N1LB );
paulineoonk 10:518a8617c86e 498 HPFLB.add( &HP1LB ).add( &HP2LB );
paulineoonk 9:285499f48cdd 499 LPFLB.add( &LP1LB ).add( &LP2LB );
paulineoonk 9:285499f48cdd 500
Gerber 31:a636a8f590a6 501 //Right Biceps
paulineoonk 10:518a8617c86e 502 NFRB.add( &N1RB );
paulineoonk 10:518a8617c86e 503 HPFRB.add( &HP1RB ).add( &HP2RB );
paulineoonk 10:518a8617c86e 504 LPFRB.add( &LP1RB ).add( &LP2RB );
paulineoonk 9:285499f48cdd 505
Gerber 31:a636a8f590a6 506 //Left Triceps
paulineoonk 10:518a8617c86e 507 NFLT.add( &N1LT );
paulineoonk 10:518a8617c86e 508 HPFLT.add( &HP1LT ).add( &HP2LT );
paulineoonk 10:518a8617c86e 509 LPFLT.add( &LP1LT ).add( &LP2LT );
paulineoonk 10:518a8617c86e 510
Gerber 31:a636a8f590a6 511 //Right Triceps
paulineoonk 10:518a8617c86e 512 NFRT.add( &N1RT );
paulineoonk 10:518a8617c86e 513 HPFRT.add( &HP1RT ).add( &HP2RT );
paulineoonk 10:518a8617c86e 514 LPFRT.add( &LP1RT ).add( &LP2RT );
paulineoonk 3:36e706d6b3d2 515
Gerber 31:a636a8f590a6 516 // serial
paulineoonk 7:05c71a859d27 517 pc.baud(115200);
paulineoonk 7:05c71a859d27 518
paulineoonk 7:05c71a859d27 519 //motor
Gerber 31:a636a8f590a6 520 M1E.period(PwmPeriod); //set PWMposition at 5000hz
Gerber 31:a636a8f590a6 521
paulineoonk 14:a861ba49107c 522 //State Machine
Gerber 24:847321a23e60 523 State = CalEMG;
Gerber 20:14edaecd7413 524 Treecko.attach(&Loop_funtion, Ts);
paulineoonk 3:36e706d6b3d2 525 while(true)
Gerber 31:a636a8f590a6 526 {}
Gerber 31:a636a8f590a6 527
paulineoonk 16:2f89d6e25782 528 }