Projectgroep 20 Biorobotics / Mbed 2 deprecated StateMachinemooi

State Machine, bezig met mooimaken

Dependencies:   Encoder HIDScope MODSERIAL biquadFilter mbed

Fork of vanEMGnaarMOTORPauline_States_nacht by Projectgroep 20 Biorobotics

main.cpp@30:b76c27e4730c, 2017-11-07 (annotated)

Committer:
Gerber
Date:
Tue Nov 07 08:54:37 2017 +0000
Revision:
30:b76c27e4730c
Parent:
29:69cc48b3feaa 
Hij geeft geen foutmeldingen!

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