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@34:87b96cc11c97, 2017-11-07 (annotated)

Committer:
Gerber
Date:
Tue Nov 07 21:55:29 2017 +0000
Revision:
34:87b96cc11c97
Parent:
33:4e5aca9f73e6 
.

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
Gerber 33:4e5aca9f73e6 360 // The values are not correctly tuned.
paulineoonk 16:2f89d6e25782 361 {
Gerber 33:4e5aca9f73e6 362 double kp = 3.36; // kind of scaled.
Gerber 32:30a36362f23d 363 double Proportional= kp*error;
paulineoonk 16:2f89d6e25782 364
Gerber 33:4e5aca9f73e6 365 double kd = 3.36; // kind of scaled.
Gerber 19:591572f4e4b5 366 double VelocityError = (error - e_prev)/Ts;
Gerber 19:591572f4e4b5 367 double Derivative = kd*VelocityError;
paulineoonk 16:2f89d6e25782 368 e_prev = error;
paulineoonk 16:2f89d6e25782 369
Gerber 33:4e5aca9f73e6 370 double ki = 4.2; // kind of scaled.
paulineoonk 16:2f89d6e25782 371 e_int = e_int+Ts*error;
Gerber 19:591572f4e4b5 372 double Integrator = ki*e_int;
paulineoonk 16:2f89d6e25782 373
Gerber 31:a636a8f590a6 374 double motorValue = (Proportional + Integrator + Derivative)/4200;
paulineoonk 16:2f89d6e25782 375 return motorValue;
paulineoonk 16:2f89d6e25782 376 }
paulineoonk 16:2f89d6e25782 377
Gerber 32:30a36362f23d 378 double FeedBackControl2(double error2, double &e_prev2, double &e_int2) // PID controller motor 2
Gerber 33:4e5aca9f73e6 379 // The values are not correctly tuned.
paulineoonk 16:2f89d6e25782 380 {
Gerber 33:4e5aca9f73e6 381 double kp2 = 3.36 // kind of scaled.
Gerber 19:591572f4e4b5 382 double Proportional2= kp2*error2;
paulineoonk 16:2f89d6e25782 383
Gerber 33:4e5aca9f73e6 384 double kd2 = 3.36; // kind of scaled.
Gerber 19:591572f4e4b5 385 double VelocityError2 = (error2 - e_prev2)/Ts;
Gerber 19:591572f4e4b5 386 double Derivative2 = kd2*VelocityError2;
paulineoonk 16:2f89d6e25782 387 e_prev2 = error2;
paulineoonk 16:2f89d6e25782 388
Gerber 33:4e5aca9f73e6 389 double ki2 = 2.1; // kind of scaled.
paulineoonk 16:2f89d6e25782 390 e_int2 = e_int2+Ts*error2;
Gerber 19:591572f4e4b5 391 double Integrator2 = ki2*e_int2;
Gerber 18:1e4f697a92cb 392
Gerber 19:591572f4e4b5 393 double motorValue2 = Proportional2 + Integrator2 + Derivative2;
paulineoonk 16:2f89d6e25782 394 return motorValue2;
paulineoonk 16:2f89d6e25782 395 }
paulineoonk 16:2f89d6e25782 396
paulineoonk 16:2f89d6e25782 397 void SetMotor1(double motorValue)
paulineoonk 16:2f89d6e25782 398 {
paulineoonk 16:2f89d6e25782 399 if (motorValue >= 0) {
paulineoonk 16:2f89d6e25782 400 M1D = 0;
paulineoonk 16:2f89d6e25782 401 }
paulineoonk 16:2f89d6e25782 402 else {
paulineoonk 16:2f89d6e25782 403 M1D = 1;
paulineoonk 16:2f89d6e25782 404 }
paulineoonk 16:2f89d6e25782 405 if (fabs(motorValue) > 1) {
Gerber 28:19cccdd68b5b 406 M1E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
paulineoonk 16:2f89d6e25782 407 }
paulineoonk 16:2f89d6e25782 408 else {
Gerber 28:19cccdd68b5b 409 M1E = fabs(motorValue); //absolute velocity determined, motor is "off" at value of 0
paulineoonk 16:2f89d6e25782 410 }
paulineoonk 16:2f89d6e25782 411 }
paulineoonk 16:2f89d6e25782 412
paulineoonk 16:2f89d6e25782 413 void SetMotor2(double motorValue2)
paulineoonk 16:2f89d6e25782 414 {
paulineoonk 16:2f89d6e25782 415 if (motorValue2 >= 0) {
Gerber 18:1e4f697a92cb 416 M2D = 1;
paulineoonk 16:2f89d6e25782 417 }
paulineoonk 16:2f89d6e25782 418 else {
Gerber 18:1e4f697a92cb 419 M2D = 0;
paulineoonk 16:2f89d6e25782 420 }
paulineoonk 16:2f89d6e25782 421 if (fabs(motorValue2) > 1) {
Gerber 28:19cccdd68b5b 422 M2E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
paulineoonk 16:2f89d6e25782 423 }
paulineoonk 16:2f89d6e25782 424 else {
Gerber 28:19cccdd68b5b 425 M2E = fabs(motorValue2); //absolute velocity determined, motor is "off" at value of 0
paulineoonk 16:2f89d6e25782 426 }
paulineoonk 16:2f89d6e25782 427 }
paulineoonk 16:2f89d6e25782 428
paulineoonk 16:2f89d6e25782 429 void MeasureAndControl(void)
paulineoonk 16:2f89d6e25782 430 {
Gerber 18:1e4f697a92cb 431 RKI();
Gerber 27:2d9f172c66ad 432 // control of 1st motor
Gerber 31:a636a8f590a6 433 double Huidigepositie = motor1.getPosition()/4200;
paulineoonk 16:2f89d6e25782 434 double error = (refP - Huidigepositie);// make an error
paulineoonk 16:2f89d6e25782 435 double motorValue = FeedBackControl(error, e_prev, e_int);
paulineoonk 16:2f89d6e25782 436 SetMotor1(motorValue);
Gerber 27:2d9f172c66ad 437 // control of 2nd motor
Gerber 31:a636a8f590a6 438 double Huidigepositie2 = motor2.getPosition()/4200;
paulineoonk 16:2f89d6e25782 439 double error2 = (refP2 - Huidigepositie2);// make an error
paulineoonk 16:2f89d6e25782 440 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
paulineoonk 16:2f89d6e25782 441 SetMotor2(motorValue2);
paulineoonk 16:2f89d6e25782 442 }
paulineoonk 7:05c71a859d27 443
paulineoonk 14:a861ba49107c 444 void Loop_funtion()
paulineoonk 14:a861ba49107c 445 {
Gerber 20:14edaecd7413 446 pc.printf("state machine begint \r\n");
Gerber 27:2d9f172c66ad 447 switch(State)
Gerber 27:2d9f172c66ad 448 {
paulineoonk 14:a861ba49107c 449 case CalEMG: // Calibration EMG
paulineoonk 15:1cfe58aea10d 450 CalibrationEMG(); //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
paulineoonk 14:a861ba49107c 451 break;
Gerber 32:30a36362f23d 452 case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
Gerber 24:847321a23e60 453 State = EMG;
Gerber 27:2d9f172c66ad 454 if (button==1) {
paulineoonk 15:1cfe58aea10d 455 State=EMG;
paulineoonk 14:a861ba49107c 456 }
Gerber 32:30a36362f23d 457 else { // button==0
Gerber 26:bfb1ae203c11 458 State=Demonstration;
Gerber 27:2d9f172c66ad 459 }
paulineoonk 14:a861ba49107c 460 break;
Gerber 27:2d9f172c66ad 461 case EMG: //Control by EMG
paulineoonk 14:a861ba49107c 462 Filteren();
paulineoonk 14:a861ba49107c 463 changePosition();
paulineoonk 16:2f89d6e25782 464 MeasureAndControl();
Gerber 31:a636a8f590a6 465 if (button==0) {
Gerber 31:a636a8f590a6 466 State=Rest;
Gerber 31:a636a8f590a6 467 }
Gerber 31:a636a8f590a6 468 else {}
paulineoonk 14:a861ba49107c 469 break;
Gerber 27:2d9f172c66ad 470 case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions.
Gerber 32:30a36362f23d 471 refP=refP;
Gerber 32:30a36362f23d 472 refP2=refP2;
Gerber 32:30a36362f23d 473 double Huidigepositie = motor1.getPosition()/4200;
Gerber 32:30a36362f23d 474 double error = (refP - Huidigepositie);// make an error
Gerber 32:30a36362f23d 475 double motorValue = FeedBackControl(error, e_prev, e_int);
Gerber 32:30a36362f23d 476 SetMotor1(motorValue);
Gerber 32:30a36362f23d 477 // control of 2nd motor
Gerber 32:30a36362f23d 478 double Huidigepositie2 = motor2.getPosition()/4200;
Gerber 32:30a36362f23d 479 double error2 = (refP2 - Huidigepositie2);// make an error
Gerber 32:30a36362f23d 480 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Gerber 32:30a36362f23d 481 SetMotor2(motorValue2);
Gerber 32:30a36362f23d 482
Gerber 31:a636a8f590a6 483 if ( button==1) {
Gerber 31:a636a8f590a6 484 State=EMG;
Gerber 31:a636a8f590a6 485 }
Gerber 31:a636a8f590a6 486 else {}
Gerber 31:a636a8f590a6 487 break;
Gerber 32:30a36362f23d 488 case Demonstration: // Control with Potmeters
Gerber 28:19cccdd68b5b 489 SetpointRobot();
Gerber 28:19cccdd68b5b 490 MeasureAndControl();
paulineoonk 14:a861ba49107c 491 break;
paulineoonk 15:1cfe58aea10d 492 }
paulineoonk 14:a861ba49107c 493 }
Miriam 0:d5fb29bc0847 494
Gerber 31:a636a8f590a6 495 int main()
Miriam 0:d5fb29bc0847 496 {
Gerber 32:30a36362f23d 497 //for filtering EMG
Gerber 31:a636a8f590a6 498 //Left Biceps
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
Gerber 31:a636a8f590a6 503 //Right Biceps
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
Gerber 31:a636a8f590a6 508 //Left Triceps
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
Gerber 31:a636a8f590a6 513 //Right Triceps
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
Gerber 31:a636a8f590a6 518 // serial
paulineoonk 7:05c71a859d27 519 pc.baud(115200);
paulineoonk 7:05c71a859d27 520
paulineoonk 7:05c71a859d27 521 //motor
Gerber 31:a636a8f590a6 522 M1E.period(PwmPeriod); //set PWMposition at 5000hz
Gerber 31:a636a8f590a6 523
paulineoonk 14:a861ba49107c 524 //State Machine
Gerber 24:847321a23e60 525 State = CalEMG;
Gerber 20:14edaecd7413 526 Treecko.attach(&Loop_funtion, Ts);
paulineoonk 3:36e706d6b3d2 527 while(true)
Gerber 31:a636a8f590a6 528 {}
Gerber 31:a636a8f590a6 529
Gerber 34:87b96cc11c97 530 }