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@27:2d9f172c66ad, 2017-11-06 (annotated)

Committer:
Gerber
Date:
Mon Nov 06 14:57:15 2017 +0000
Revision:
27:2d9f172c66ad
Parent:
26:bfb1ae203c11
Child:
28:19cccdd68b5b
State Machine mooi maken

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