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@22:02a9b5914cc7, 2017-11-03 (annotated)

Committer:
Gerber
Date:
Fri Nov 03 04:28:04 2017 +0000
Revision:
22:02a9b5914cc7
Parent:
21:9307dc9be4f7
Child:
23:08255478f6cd
DEMO met potmeter en alles toegevoegd.

; We vinden de robot zo lief en hebben vertrouwen in hem :)... <3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Miriam 0:d5fb29bc0847 1 //libaries
Miriam 0:d5fb29bc0847 2 #include "mbed.h"
Miriam 0:d5fb29bc0847 3 #include "BiQuad.h"
Miriam 0:d5fb29bc0847 4 #include "HIDScope.h"
Miriam 0:d5fb29bc0847 5 #include "encoder.h"
Miriam 0:d5fb29bc0847 6 #include "MODSERIAL.h"
Miriam 0:d5fb29bc0847 7
paulineoonk 15:1cfe58aea10d 8 //State Machine en calibratie
Gerber 22:02a9b5914cc7 9 enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo, STOP};
paulineoonk 15:1cfe58aea10d 10 States State;
paulineoonk 15:1cfe58aea10d 11 int Counter;
paulineoonk 14:a861ba49107c 12 bool Position_controller_on;
paulineoonk 15:1cfe58aea10d 13 double looptime = 0.002f;
paulineoonk 15:1cfe58aea10d 14 double value;
paulineoonk 15:1cfe58aea10d 15 double home1;
Gerber 18:1e4f697a92cb 16 DigitalIn button (D1);
Gerber 22:02a9b5914cc7 17 bool automode=false;
paulineoonk 15:1cfe58aea10d 18
paulineoonk 15:1cfe58aea10d 19 //Encoder/motor
paulineoonk 15:1cfe58aea10d 20 double Huidigepositie1;
paulineoonk 15:1cfe58aea10d 21 double Huidigepositie2;
paulineoonk 15:1cfe58aea10d 22 double motorValue1;
paulineoonk 15:1cfe58aea10d 23 double motorValue2;
paulineoonk 14:a861ba49107c 24
paulineoonk 3:36e706d6b3d2 25 //globalvariables Motor
paulineoonk 3:36e706d6b3d2 26 Ticker Treecko; //We make a awesome ticker for our control system
paulineoonk 8:c4ec359af35d 27 Ticker printer;
paulineoonk 16:2f89d6e25782 28 PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed
paulineoonk 16:2f89d6e25782 29 PwmOut M2E(D5);
paulineoonk 16:2f89d6e25782 30 DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control
paulineoonk 16:2f89d6e25782 31 Encoder motor1(D13,D12,true);
paulineoonk 16:2f89d6e25782 32 Encoder motor2(D9,D8,true);
paulineoonk 16:2f89d6e25782 33 DigitalOut M2D(D4);
Miriam 0:d5fb29bc0847 34
paulineoonk 16:2f89d6e25782 35 //DEMO
paulineoonk 16:2f89d6e25782 36 AnalogIn potMeter2(A1);
paulineoonk 16:2f89d6e25782 37 AnalogIn potMeter1(A2);
paulineoonk 16:2f89d6e25782 38
paulineoonk 3:36e706d6b3d2 39 MODSERIAL pc(USBTX,USBRX);
paulineoonk 3:36e706d6b3d2 40
paulineoonk 16:2f89d6e25782 41 double PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
Gerber 20:14edaecd7413 42 const double Ts = 0.002f; // tickettijd/ sample time
paulineoonk 16:2f89d6e25782 43 double e_prev = 0;
paulineoonk 16:2f89d6e25782 44 double e_int = 0;
paulineoonk 16:2f89d6e25782 45 double e_prev2 = 0;
paulineoonk 16:2f89d6e25782 46 double e_int2 = 0;
paulineoonk 16:2f89d6e25782 47
paulineoonk 6:e0e5da2c068f 48 double tijdstap = 0.002;
paulineoonk 8:c4ec359af35d 49 volatile double LBF;
paulineoonk 8:c4ec359af35d 50 volatile double RBF;
paulineoonk 8:c4ec359af35d 51 volatile double LTF;
paulineoonk 8:c4ec359af35d 52 volatile double RTF;
Miriam 0:d5fb29bc0847 53
paulineoonk 7:05c71a859d27 54 //buttons en leds voor calibration
paulineoonk 7:05c71a859d27 55 DigitalIn button1(PTA4);
paulineoonk 9:285499f48cdd 56
Gerber 18:1e4f697a92cb 57 DigitalOut led(D2);
paulineoonk 8:c4ec359af35d 58
paulineoonk 10:518a8617c86e 59 //MVC for calibration
paulineoonk 10:518a8617c86e 60 double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
paulineoonk 10:518a8617c86e 61 //MEAN for calibration - rest
paulineoonk 10:518a8617c86e 62 double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;
paulineoonk 9:285499f48cdd 63
paulineoonk 10:518a8617c86e 64 double emgMEANSUBLB;double emgMEANSUBRB ;double emgMEANSUBLT ;double emgMEANSUBRT ;
paulineoonk 10:518a8617c86e 65 double emgSUMLB;double emgSUMRB;double emgSUMLT;double emgSUMRT;
paulineoonk 10:518a8617c86e 66
paulineoonk 8:c4ec359af35d 67
paulineoonk 7:05c71a859d27 68 bool caldone = false;
paulineoonk 8:c4ec359af35d 69 int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
paulineoonk 8:c4ec359af35d 70
paulineoonk 9:285499f48cdd 71 int Timescalibration = 0;
paulineoonk 10:518a8617c86e 72 int TimescalibrationREST = 0;
paulineoonk 10:518a8617c86e 73
Miriam 0:d5fb29bc0847 74
paulineoonk 9:285499f48cdd 75 // Biquad filters voor Left Bicep (LB)
paulineoonk 9:285499f48cdd 76 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 77 BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 78 BiQuadChain NFLB;
paulineoonk 9:285499f48cdd 79 BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 80 BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 81 BiQuadChain HPFLB;
paulineoonk 9:285499f48cdd 82 BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 83 BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 84 BiQuadChain LPFLB;
paulineoonk 9:285499f48cdd 85
paulineoonk 9:285499f48cdd 86 // Biquad filters voor Right Bicep (RB)
Miriam 0:d5fb29bc0847 87 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 88 BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 89 BiQuadChain NFRB;
paulineoonk 9:285499f48cdd 90 BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 91 BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 92 BiQuadChain HPFRB;
paulineoonk 9:285499f48cdd 93 BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 94 BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 95 BiQuadChain LPFRB;
paulineoonk 9:285499f48cdd 96
paulineoonk 9:285499f48cdd 97 // Biquad filters voor Left Tricep (LT)
paulineoonk 9:285499f48cdd 98 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 99 BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 100 BiQuadChain NFLT;
paulineoonk 9:285499f48cdd 101 BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 102 BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 103 BiQuadChain HPFLT;
paulineoonk 9:285499f48cdd 104 BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 105 BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 106 BiQuadChain LPFLT;
paulineoonk 9:285499f48cdd 107
paulineoonk 9:285499f48cdd 108 // Biquad filters voor Left Tricep (RT)
paulineoonk 9:285499f48cdd 109 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
paulineoonk 9:285499f48cdd 110 BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
paulineoonk 9:285499f48cdd 111 BiQuadChain NFRT;
paulineoonk 9:285499f48cdd 112 BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 113 BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
paulineoonk 9:285499f48cdd 114 BiQuadChain HPFRT;
paulineoonk 9:285499f48cdd 115 BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
paulineoonk 9:285499f48cdd 116 BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
paulineoonk 9:285499f48cdd 117 BiQuadChain LPFRT;
paulineoonk 9:285499f48cdd 118
paulineoonk 15:1cfe58aea10d 119 Timer LooptimeEMG; //moetuiteindelijk weg
paulineoonk 6:e0e5da2c068f 120
paulineoonk 7:05c71a859d27 121 //filters
paulineoonk 8:c4ec359af35d 122 double emgNotchLB;
paulineoonk 8:c4ec359af35d 123 double emgHPLB;
paulineoonk 8:c4ec359af35d 124 double emgAbsHPLB;
paulineoonk 8:c4ec359af35d 125 double emgLPLB;
paulineoonk 8:c4ec359af35d 126
paulineoonk 8:c4ec359af35d 127 double emgNotchRB;
paulineoonk 8:c4ec359af35d 128 double emgHPRB;
paulineoonk 8:c4ec359af35d 129 double emgAbsHPRB;
paulineoonk 8:c4ec359af35d 130 double emgLPRB;
paulineoonk 7:05c71a859d27 131
paulineoonk 8:c4ec359af35d 132 double emgNotchLT;
paulineoonk 8:c4ec359af35d 133 double emgHPLT;
paulineoonk 8:c4ec359af35d 134 double emgAbsHPLT;
paulineoonk 8:c4ec359af35d 135 double emgLPLT;
paulineoonk 7:05c71a859d27 136
paulineoonk 8:c4ec359af35d 137 double emgNotchRT;
paulineoonk 8:c4ec359af35d 138 double emgHPRT;
paulineoonk 8:c4ec359af35d 139 double emgAbsHPRT;
paulineoonk 8:c4ec359af35d 140 double emgLPRT;
Miriam 0:d5fb29bc0847 141
paulineoonk 3:36e706d6b3d2 142 double f = 500; // frequency
paulineoonk 3:36e706d6b3d2 143 double dt = 1/f; // sample frequency
paulineoonk 8:c4ec359af35d 144
paulineoonk 8:c4ec359af35d 145 AnalogIn emgLB(A0); // EMG lezen
paulineoonk 8:c4ec359af35d 146 AnalogIn emgRB(A1);
paulineoonk 8:c4ec359af35d 147 AnalogIn emgLT(A2);
paulineoonk 8:c4ec359af35d 148 AnalogIn emgRT(A3);
Miriam 0:d5fb29bc0847 149
paulineoonk 16:2f89d6e25782 150 //double MVCLB = 0.3;
paulineoonk 16:2f89d6e25782 151 //double MVCRB = 0.3;
paulineoonk 16:2f89d6e25782 152 //double MVCLT = 0.3;
paulineoonk 16:2f89d6e25782 153 //double MVCRT = 0.3;
paulineoonk 8:c4ec359af35d 154
paulineoonk 11:b46a4c48c08f 155 // variabelen changePosition
paulineoonk 11:b46a4c48c08f 156 int goalx, goaly;
paulineoonk 11:b46a4c48c08f 157
Gerber 18:1e4f697a92cb 158 // variables RKI
Gerber 18:1e4f697a92cb 159 double pi = 3.14159265359;
Gerber 18:1e4f697a92cb 160 double q1 = (pi/2); //Reference position hoek 1 in radiance
Gerber 18:1e4f697a92cb 161 double q2 = -(pi/2); //Reference position hoek 2 in radiance
Gerber 18:1e4f697a92cb 162 const double L1 = 0.30; //Length arm 1 in mm
Gerber 18:1e4f697a92cb 163 const double L2 = 0.38; //Length arm 2 in mm
Gerber 18:1e4f697a92cb 164 double B1 = 1; //Friction constant motor 1
Gerber 18:1e4f697a92cb 165 double B2 = 1; //Friction constant motor 2
Gerber 18:1e4f697a92cb 166 double K = 1; //Spring constant movement from end-effector position to setpoint position
Gerber 18:1e4f697a92cb 167 double Tijd = 1; //Timestep value
Gerber 18:1e4f697a92cb 168 double Rsx = 0.38; //Reference x-component of the setpoint radius
Gerber 18:1e4f697a92cb 169 double Rsy = 0.30; //Reference y-component of the setpoint radius
Gerber 18:1e4f697a92cb 170 double refP = 0; //Reference position motor 1
Gerber 18:1e4f697a92cb 171 double refP2 = 0.5*pi; //Reference position motor 2
Gerber 18:1e4f697a92cb 172 double Rex = cos(q1)*L1 - sin(q2)*L2; //The x-component of the end-effector radius
Gerber 18:1e4f697a92cb 173 double Rey = sin(q1)*L1 + cos(q2)*L2; //The y-component of the end-effector radius
Gerber 18:1e4f697a92cb 174 double R1x = 0; //The x-component of the joint 1 radius
Gerber 18:1e4f697a92cb 175 double R1y = 0; //The y-component of the joint 1 radius
Gerber 18:1e4f697a92cb 176 double R2x = cos(q1)*L1; //The x-component of the joint 2 radius
Gerber 18:1e4f697a92cb 177 double R2y = sin(q1)*L1; //The y-component of the joint 1 radius
Gerber 18:1e4f697a92cb 178 double Fx = 0;
Gerber 18:1e4f697a92cb 179 double Fy = 0;
Gerber 18:1e4f697a92cb 180 double Tor1 = 0;
Gerber 18:1e4f697a92cb 181 double Tor2 = 0;
Gerber 18:1e4f697a92cb 182 double w1= 0;
Gerber 18:1e4f697a92cb 183 double w2= 0;
Gerber 18:1e4f697a92cb 184
paulineoonk 8:c4ec359af35d 185 void Filteren()
Miriam 0:d5fb29bc0847 186 {
paulineoonk 15:1cfe58aea10d 187 // LooptimeEMG.reset();
paulineoonk 15:1cfe58aea10d 188 // LooptimeEMG.start();
paulineoonk 8:c4ec359af35d 189
paulineoonk 8:c4ec359af35d 190 //EMG 1
paulineoonk 8:c4ec359af35d 191
paulineoonk 10:518a8617c86e 192 emgNotchLB = NFLB.step(emgLB.read() ); // Notch filter
paulineoonk 10:518a8617c86e 193 emgHPLB = HPFLB.step(emgNotchLB); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 194 emgAbsHPLB = abs(emgHPLB); // Take absolute value
paulineoonk 10:518a8617c86e 195 emgLPLB = LPFLB.step(emgAbsHPLB); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 196 emgMEANSUBLB = emgLPLB - RESTMEANLB; //substract the restmean value
paulineoonk 10:518a8617c86e 197 LBF = emgLPLB/MVCLB; // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
paulineoonk 9:285499f48cdd 198
paulineoonk 10:518a8617c86e 199 emgNotchRB = NFRB.step(emgRB.read()); // Notch filter
paulineoonk 10:518a8617c86e 200 emgHPRB = HPFRB.step(emgNotchRB); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 201 emgAbsHPRB = abs(emgHPRB); // Take absolute value
paulineoonk 10:518a8617c86e 202 emgLPRB = LPFRB.step(emgAbsHPRB); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 203 emgMEANSUBLB = emgLPLB - RESTMEANLB;
paulineoonk 9:285499f48cdd 204 RBF = emgLPRB/MVCRB; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 205
paulineoonk 10:518a8617c86e 206 emgNotchLT = NFLT.step(emgLT.read() ); // Notch filter
paulineoonk 10:518a8617c86e 207 emgHPLT = HPFLT.step(emgNotchLT); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 208 emgAbsHPLT = abs(emgHPLT); // Take absolute value
paulineoonk 10:518a8617c86e 209 emgLPLT = LPFLT.step(emgAbsHPLT); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 210 emgMEANSUBLT = emgLPLT - RESTMEANLT; //substract the restmean value
paulineoonk 9:285499f48cdd 211 LTF = emgLPLT/MVCLT; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 212
paulineoonk 10:518a8617c86e 213 emgNotchRT = NFRT.step(emgRT.read() ); // Notch filter
paulineoonk 10:518a8617c86e 214 emgHPRT = HPFRT.step(emgNotchRT); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 215 emgAbsHPRT = abs(emgHPRT); // Take absolute value
paulineoonk 10:518a8617c86e 216 emgLPRT = LPFRT.step(emgAbsHPRT); // Low-pass filter: creates envelope
paulineoonk 10:518a8617c86e 217 emgMEANSUBRT = emgLPRT - RESTMEANRT; //substract the restmean value
paulineoonk 9:285499f48cdd 218 RTF = emgLPRT/MVCRT; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 219
paulineoonk 8:c4ec359af35d 220 //if (emgFiltered >1)
paulineoonk 8:c4ec359af35d 221 //{
paulineoonk 8:c4ec359af35d 222 // emgFiltered=1.00;
paulineoonk 8:c4ec359af35d 223 //}
paulineoonk 8:c4ec359af35d 224 //pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n, emgreadRB = %f , emgFiltered = %f, maxi = %f \r\n, emgreadLT = %f , emgFiltered = %f, maxi = %f \r\n, emgreadRT = %f , emgFiltered = %f, maxi = %f \r\n",emgLB.read(), LBF, maxiLB,looptime.read(),emgRB.read(), RBF, maxiRB,emgLT.read(), LTF, maxiLT, emgRT.read(), RTF, maxiRT);
paulineoonk 6:e0e5da2c068f 225 //int maxwaarde = 4096; // = 64x64
paulineoonk 6:e0e5da2c068f 226 //double refP = emgFiltered*maxwaarde;
paulineoonk 6:e0e5da2c068f 227 //return refP; // value between 0 and 4096
paulineoonk 9:285499f48cdd 228
Miriam 0:d5fb29bc0847 229 }
paulineoonk 9:285499f48cdd 230
paulineoonk 10:518a8617c86e 231 void CalibrationEMG()
paulineoonk 7:05c71a859d27 232 {
Gerber 20:14edaecd7413 233 pc.printf("Timescalibration = %i \r\n",Timescalibration);
paulineoonk 7:05c71a859d27 234 Timescalibration++;
paulineoonk 9:285499f48cdd 235
paulineoonk 10:518a8617c86e 236 if(Timescalibration<2000)
paulineoonk 9:285499f48cdd 237 {
Gerber 20:14edaecd7413 238 pc.printf("calibratie rust EMG \r\n");
charloverwijk 17:dbdbd1edc260 239 led = 1;
Gerber 20:14edaecd7413 240 // wait(0.2);
Gerber 20:14edaecd7413 241 //led = 0;
paulineoonk 10:518a8617c86e 242
paulineoonk 10:518a8617c86e 243 emgNotchLB = NFLB.step(emgLB.read() );
paulineoonk 10:518a8617c86e 244 emgHPLB = HPFLB.step(emgNotchLB);
paulineoonk 10:518a8617c86e 245 emgAbsHPLB = abs(emgHPLB);
paulineoonk 10:518a8617c86e 246 emgLPLB = LPFLB.step(emgAbsHPLB);
paulineoonk 10:518a8617c86e 247 emgSUMLB += emgLPLB; //SUM all rest values LB
paulineoonk 10:518a8617c86e 248
paulineoonk 11:b46a4c48c08f 249 emgNotchRB = NFRB.step(emgRB.read());
paulineoonk 10:518a8617c86e 250 emgHPRB = HPFRB.step(emgNotchRB);
paulineoonk 10:518a8617c86e 251 emgAbsHPRB = abs(emgHPRB);
paulineoonk 10:518a8617c86e 252 emgLPRB = LPFRB.step(emgAbsHPRB);
paulineoonk 10:518a8617c86e 253 emgSUMRB += emgLPRB; //SUM all rest values RB
paulineoonk 10:518a8617c86e 254
paulineoonk 10:518a8617c86e 255 emgNotchLT = NFLT.step(emgLT.read() );
paulineoonk 10:518a8617c86e 256 emgHPLT = HPFLT.step(emgNotchLT);
paulineoonk 10:518a8617c86e 257 emgAbsHPLT = abs(emgHPLT);
paulineoonk 10:518a8617c86e 258 emgLPLT = LPFLT.step(emgAbsHPLT);
paulineoonk 10:518a8617c86e 259 emgSUMLT += emgLPLT; //SUM all rest values LT
paulineoonk 10:518a8617c86e 260
paulineoonk 10:518a8617c86e 261 emgNotchRT = NFRT.step(emgRT.read() );
paulineoonk 10:518a8617c86e 262 emgHPRT = HPFRT.step(emgNotchRT);
paulineoonk 10:518a8617c86e 263 emgAbsHPRT = abs(emgHPRT);
paulineoonk 10:518a8617c86e 264 emgLPRT = LPFRT.step(emgAbsHPRT);
paulineoonk 10:518a8617c86e 265 emgSUMRT += emgLPRT; //SUM all rest values RT
paulineoonk 10:518a8617c86e 266 }
paulineoonk 10:518a8617c86e 267 if(Timescalibration==1999)
paulineoonk 10:518a8617c86e 268 {
Gerber 20:14edaecd7413 269 /*led = 1;
charloverwijk 17:dbdbd1edc260 270 wait(0.2);
charloverwijk 17:dbdbd1edc260 271 led = 0;
charloverwijk 17:dbdbd1edc260 272 wait(0.2);
charloverwijk 17:dbdbd1edc260 273 led = 1;
charloverwijk 17:dbdbd1edc260 274 wait(0.2);
Gerber 20:14edaecd7413 275 */
charloverwijk 17:dbdbd1edc260 276 led = 0;
paulineoonk 10:518a8617c86e 277 RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 278 RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 279 RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 280 RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
paulineoonk 10:518a8617c86e 281 }
Gerber 21:9307dc9be4f7 282 if(Timescalibration>2000 && Timescalibration<3000)
paulineoonk 10:518a8617c86e 283 {
Gerber 20:14edaecd7413 284 pc.printf("maximum linker biceps \r\n");
charloverwijk 17:dbdbd1edc260 285 led = 1;
Gerber 20:14edaecd7413 286 /*wait(0.2);
charloverwijk 17:dbdbd1edc260 287 led = 0;
charloverwijk 17:dbdbd1edc260 288 wait(0.2);
charloverwijk 17:dbdbd1edc260 289 led = 1;
charloverwijk 17:dbdbd1edc260 290 wait(0.2);
charloverwijk 17:dbdbd1edc260 291 led = 0;
charloverwijk 17:dbdbd1edc260 292 wait(0.2);
charloverwijk 17:dbdbd1edc260 293 led = 1;
charloverwijk 17:dbdbd1edc260 294 wait(0.2);
charloverwijk 17:dbdbd1edc260 295 led = 0;
Gerber 20:14edaecd7413 296 */
paulineoonk 10:518a8617c86e 297 emgNotchLB = NFLB.step(emgLB.read() );
paulineoonk 10:518a8617c86e 298 emgHPLB = HPFLB.step(emgNotchLB);
paulineoonk 10:518a8617c86e 299 emgAbsHPLB = abs(emgHPLB);
paulineoonk 10:518a8617c86e 300 emgLPLB = LPFLB.step(emgAbsHPLB);
paulineoonk 9:285499f48cdd 301 double emgfinalLB = emgLPLB;
paulineoonk 9:285499f48cdd 302 if (emgfinalLB > MVCLB)
paulineoonk 9:285499f48cdd 303 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 304 MVCLB = emgfinalLB;
paulineoonk 9:285499f48cdd 305 }
paulineoonk 9:285499f48cdd 306 }
paulineoonk 9:285499f48cdd 307
Gerber 21:9307dc9be4f7 308 if(Timescalibration>3000 && Timescalibration<4000)
paulineoonk 9:285499f48cdd 309 {
Gerber 20:14edaecd7413 310 pc.printf(" maximum rechter biceps \r\n");
Gerber 20:14edaecd7413 311 /*led = 1;
charloverwijk 17:dbdbd1edc260 312 wait(0.2);
charloverwijk 17:dbdbd1edc260 313 led = 0;
charloverwijk 17:dbdbd1edc260 314 wait(0.2);
charloverwijk 17:dbdbd1edc260 315 led = 1;
charloverwijk 17:dbdbd1edc260 316 wait(0.2);
charloverwijk 17:dbdbd1edc260 317 led = 0;
charloverwijk 17:dbdbd1edc260 318 wait(0.2);
charloverwijk 17:dbdbd1edc260 319 led = 1;
charloverwijk 17:dbdbd1edc260 320 wait(0.2);
charloverwijk 17:dbdbd1edc260 321 led = 0;
charloverwijk 17:dbdbd1edc260 322 wait(0.2);
charloverwijk 17:dbdbd1edc260 323 led = 1;
charloverwijk 17:dbdbd1edc260 324 wait(0.2);
Gerber 20:14edaecd7413 325 */
charloverwijk 17:dbdbd1edc260 326 led = 0;
paulineoonk 10:518a8617c86e 327 emgNotchRB = NFRB.step(emgRB.read());
paulineoonk 10:518a8617c86e 328 emgHPRB = HPFRB.step(emgNotchRB);
paulineoonk 10:518a8617c86e 329 emgAbsHPRB = abs(emgHPRB);
paulineoonk 10:518a8617c86e 330 emgLPRB = LPFRB.step(emgAbsHPRB);
paulineoonk 9:285499f48cdd 331 double emgfinalRB = emgLPRB;
paulineoonk 9:285499f48cdd 332 if (emgfinalRB > MVCRB)
paulineoonk 7:05c71a859d27 333 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 334 MVCRB = emgfinalRB;
paulineoonk 9:285499f48cdd 335 }
paulineoonk 9:285499f48cdd 336 }
paulineoonk 9:285499f48cdd 337
Gerber 21:9307dc9be4f7 338 if(Timescalibration>4000 && Timescalibration<5000)
paulineoonk 9:285499f48cdd 339 {
Gerber 20:14edaecd7413 340 pc.printf("maximum linker triceps \r\n");
charloverwijk 17:dbdbd1edc260 341 led = 1;
Gerber 20:14edaecd7413 342 /*wait(0.2);
charloverwijk 17:dbdbd1edc260 343 led = 0;
charloverwijk 17:dbdbd1edc260 344 wait(0.2);
charloverwijk 17:dbdbd1edc260 345 led = 1;
charloverwijk 17:dbdbd1edc260 346 wait(0.2);
charloverwijk 17:dbdbd1edc260 347 led = 0;
charloverwijk 17:dbdbd1edc260 348 wait(0.2);
charloverwijk 17:dbdbd1edc260 349 led = 1;
charloverwijk 17:dbdbd1edc260 350 wait(0.2);
charloverwijk 17:dbdbd1edc260 351 led = 0;
charloverwijk 17:dbdbd1edc260 352 wait(0.2);
charloverwijk 17:dbdbd1edc260 353 led = 1;
charloverwijk 17:dbdbd1edc260 354 wait(0.2);
charloverwijk 17:dbdbd1edc260 355 led = 0;
charloverwijk 17:dbdbd1edc260 356 wait(0.2);
charloverwijk 17:dbdbd1edc260 357 led = 1;
charloverwijk 17:dbdbd1edc260 358 wait(0.2);
charloverwijk 17:dbdbd1edc260 359 led = 0;
Gerber 20:14edaecd7413 360 */
paulineoonk 10:518a8617c86e 361 emgNotchLT = NFLT.step(emgLT.read() );
paulineoonk 10:518a8617c86e 362 emgHPLT = HPFLT.step(emgNotchLT);
paulineoonk 10:518a8617c86e 363 emgAbsHPLT = abs(emgHPLT);
paulineoonk 10:518a8617c86e 364 emgLPLT = LPFLT.step(emgAbsHPLT);
paulineoonk 9:285499f48cdd 365 double emgfinalLT = emgLPLT;
paulineoonk 9:285499f48cdd 366 if (emgfinalLT > MVCLT)
paulineoonk 9:285499f48cdd 367 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 368 MVCLT = emgfinalLT;
paulineoonk 7:05c71a859d27 369 }
paulineoonk 9:285499f48cdd 370 }
paulineoonk 9:285499f48cdd 371
Gerber 21:9307dc9be4f7 372 if(Timescalibration>5000 && Timescalibration<6000)
paulineoonk 9:285499f48cdd 373 {
Gerber 20:14edaecd7413 374 pc.printf("maximum rechter triceps");
paulineoonk 10:518a8617c86e 375 emgNotchRT = NFRT.step(emgRT.read() );
paulineoonk 10:518a8617c86e 376 emgHPRT = HPFRT.step(emgNotchRT);
paulineoonk 10:518a8617c86e 377 emgAbsHPRT = abs(emgHPRT);
paulineoonk 10:518a8617c86e 378 emgLPRT = LPFRT.step(emgAbsHPRT);
paulineoonk 9:285499f48cdd 379 double emgfinalRT = emgLPRT;
paulineoonk 9:285499f48cdd 380 if (emgfinalRT > MVCRT)
paulineoonk 9:285499f48cdd 381 { //determine what the highest reachable emg signal is
paulineoonk 9:285499f48cdd 382 MVCRT = emgfinalRT;
paulineoonk 9:285499f48cdd 383 }
paulineoonk 9:285499f48cdd 384 }
paulineoonk 9:285499f48cdd 385
Gerber 21:9307dc9be4f7 386 if(Timescalibration>6000)
paulineoonk 7:05c71a859d27 387 {
Gerber 20:14edaecd7413 388 pc.printf("calibratie afgelopen");
Gerber 22:02a9b5914cc7 389 State = Demo;
paulineoonk 7:05c71a859d27 390 }
paulineoonk 9:285499f48cdd 391 // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
paulineoonk 9:285499f48cdd 392 //}
paulineoonk 7:05c71a859d27 393 //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
paulineoonk 7:05c71a859d27 394 //return maxi;
paulineoonk 7:05c71a859d27 395 }
paulineoonk 16:2f89d6e25782 396
Gerber 18:1e4f697a92cb 397 void RKI()
Gerber 18:1e4f697a92cb 398 {
Gerber 18:1e4f697a92cb 399 Rex = cos(q1)*L1 - sin(q2)*L2;
Gerber 18:1e4f697a92cb 400 Rey = sin(q1)*L1 + cos(q2)*L2;
Gerber 18:1e4f697a92cb 401 R2x = cos(q1)*L1;
Gerber 18:1e4f697a92cb 402 R2y = sin(q1)*L1;
Gerber 18:1e4f697a92cb 403 Fx = (Rsx-Rex)*K;
Gerber 18:1e4f697a92cb 404 Fy = (Rsy-Rey)*K;
Gerber 18:1e4f697a92cb 405 Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx;
Gerber 18:1e4f697a92cb 406 Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx;
Gerber 18:1e4f697a92cb 407 w1 = Tor1/B1;
Gerber 18:1e4f697a92cb 408 w2 = Tor2/B2;
Gerber 18:1e4f697a92cb 409 q1 = q1 + w1*Tijd;
Gerber 18:1e4f697a92cb 410 q2 = q2 + w2*Tijd;
Gerber 18:1e4f697a92cb 411
Gerber 18:1e4f697a92cb 412 int maxwaarde = 4096; // = 64x64
Gerber 18:1e4f697a92cb 413 refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
Gerber 18:1e4f697a92cb 414 refP2 = (((-pi) + q1 - q2)/(2*pi))*maxwaarde; //Get reference positions was eerst 0.5*pi
Gerber 18:1e4f697a92cb 415 }
Gerber 18:1e4f697a92cb 416
Gerber 19:591572f4e4b5 417 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 418 {
Gerber 19:591572f4e4b5 419 double kp = 0.0015; // kind of scaled.
Gerber 19:591572f4e4b5 420 double Proportional= kp*error;
paulineoonk 16:2f89d6e25782 421
Gerber 19:591572f4e4b5 422 double kd = 0.000008; // kind of scaled.
Gerber 19:591572f4e4b5 423 double VelocityError = (error - e_prev)/Ts;
Gerber 19:591572f4e4b5 424 double Derivative = kd*VelocityError;
paulineoonk 16:2f89d6e25782 425 e_prev = error;
paulineoonk 16:2f89d6e25782 426
Gerber 19:591572f4e4b5 427 double ki = 0.0001; // kind of scaled.
paulineoonk 16:2f89d6e25782 428 e_int = e_int+Ts*error;
Gerber 19:591572f4e4b5 429 double Integrator = ki*e_int;
paulineoonk 16:2f89d6e25782 430
Gerber 18:1e4f697a92cb 431
Gerber 19:591572f4e4b5 432 double motorValue = Proportional + Integrator + Derivative;
paulineoonk 16:2f89d6e25782 433 return motorValue;
paulineoonk 16:2f89d6e25782 434 }
paulineoonk 16:2f89d6e25782 435
Gerber 19:591572f4e4b5 436 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 437 {
Gerber 19:591572f4e4b5 438 double kp2 = 0.002; // kind of scaled.
Gerber 19:591572f4e4b5 439 double Proportional2= kp2*error2;
paulineoonk 16:2f89d6e25782 440
Gerber 19:591572f4e4b5 441 double kd2 = 0.000008; // kind of scaled.
Gerber 19:591572f4e4b5 442 double VelocityError2 = (error2 - e_prev2)/Ts;
Gerber 19:591572f4e4b5 443 double Derivative2 = kd2*VelocityError2;
paulineoonk 16:2f89d6e25782 444 e_prev2 = error2;
paulineoonk 16:2f89d6e25782 445
Gerber 19:591572f4e4b5 446 double ki2 = 0.00005; // kind of scaled.
paulineoonk 16:2f89d6e25782 447 e_int2 = e_int2+Ts*error2;
Gerber 19:591572f4e4b5 448 double Integrator2 = ki2*e_int2;
Gerber 18:1e4f697a92cb 449
paulineoonk 16:2f89d6e25782 450
Gerber 19:591572f4e4b5 451 double motorValue2 = Proportional2 + Integrator2 + Derivative2;
paulineoonk 16:2f89d6e25782 452 return motorValue2;
Gerber 18:1e4f697a92cb 453
paulineoonk 16:2f89d6e25782 454 }
paulineoonk 16:2f89d6e25782 455
paulineoonk 16:2f89d6e25782 456 void SetMotor1(double motorValue)
paulineoonk 16:2f89d6e25782 457 {
paulineoonk 16:2f89d6e25782 458 if (motorValue >= 0) {
paulineoonk 16:2f89d6e25782 459 M1D = 0;
paulineoonk 16:2f89d6e25782 460 }
paulineoonk 16:2f89d6e25782 461 else {
paulineoonk 16:2f89d6e25782 462 M1D = 1;
paulineoonk 16:2f89d6e25782 463 }
paulineoonk 16:2f89d6e25782 464
paulineoonk 16:2f89d6e25782 465 if (fabs(motorValue) > 1) {
paulineoonk 16:2f89d6e25782 466 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
paulineoonk 16:2f89d6e25782 467 }
paulineoonk 16:2f89d6e25782 468 else {
paulineoonk 16:2f89d6e25782 469 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
paulineoonk 16:2f89d6e25782 470 }
paulineoonk 16:2f89d6e25782 471 }
paulineoonk 16:2f89d6e25782 472
paulineoonk 16:2f89d6e25782 473 void SetMotor2(double motorValue2)
paulineoonk 16:2f89d6e25782 474 {
paulineoonk 16:2f89d6e25782 475 if (motorValue2 >= 0) {
Gerber 18:1e4f697a92cb 476 M2D = 1;
paulineoonk 16:2f89d6e25782 477 }
paulineoonk 16:2f89d6e25782 478 else {
Gerber 18:1e4f697a92cb 479 M2D = 0;
paulineoonk 16:2f89d6e25782 480 }
paulineoonk 16:2f89d6e25782 481
paulineoonk 16:2f89d6e25782 482 if (fabs(motorValue2) > 1) {
paulineoonk 16:2f89d6e25782 483 M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
paulineoonk 16:2f89d6e25782 484 }
paulineoonk 16:2f89d6e25782 485 else {
paulineoonk 16:2f89d6e25782 486 M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
paulineoonk 16:2f89d6e25782 487 }
paulineoonk 16:2f89d6e25782 488 }
paulineoonk 16:2f89d6e25782 489
Gerber 22:02a9b5914cc7 490 void changePosition () // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
Gerber 22:02a9b5914cc7 491 {
Gerber 22:02a9b5914cc7 492 if (RBF>0.5) {
Gerber 22:02a9b5914cc7 493 //goalx++; // hoe veel verder gaat hij? 1 cm? 10 cm?
Gerber 22:02a9b5914cc7 494 Rsx += 0.001;
Gerber 22:02a9b5914cc7 495 }
Gerber 22:02a9b5914cc7 496 else if (RTF>0.5) {
Gerber 22:02a9b5914cc7 497 //goalx--;
Gerber 22:02a9b5914cc7 498 Rsx -= 0.001;
Gerber 22:02a9b5914cc7 499 }
Gerber 22:02a9b5914cc7 500 else {}
Gerber 22:02a9b5914cc7 501 if (LBF>0.5) {
Gerber 22:02a9b5914cc7 502 //goaly++;
Gerber 22:02a9b5914cc7 503 Rsy +=0.001;
Gerber 22:02a9b5914cc7 504 }
Gerber 22:02a9b5914cc7 505 else if (LTF>0.5) {
Gerber 22:02a9b5914cc7 506 //goaly--;
Gerber 22:02a9b5914cc7 507 Rsy -= 0.001;
Gerber 22:02a9b5914cc7 508 }
Gerber 22:02a9b5914cc7 509 else {}
Gerber 22:02a9b5914cc7 510 pc.printf("goalx = %f, goaly = %f\r\n",Rsx, Rsy);
Gerber 22:02a9b5914cc7 511 }
Gerber 22:02a9b5914cc7 512
Gerber 22:02a9b5914cc7 513 void DemoPotmeter ()
Gerber 22:02a9b5914cc7 514 {
Gerber 22:02a9b5914cc7 515 double Potmeterwaarde2 = potMeter2.read();
Gerber 22:02a9b5914cc7 516 double Potmeterwaarde1 = potMeter1.read();
Gerber 22:02a9b5914cc7 517
Gerber 22:02a9b5914cc7 518 if (Potmeterwaarde2>0.6) {
Gerber 22:02a9b5914cc7 519 Rsx += 0.001; //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
Gerber 22:02a9b5914cc7 520 }
Gerber 22:02a9b5914cc7 521 else if (Potmeterwaarde2<0.4) {
Gerber 22:02a9b5914cc7 522 Rsx -= 0.001; //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
Gerber 22:02a9b5914cc7 523 }
Gerber 22:02a9b5914cc7 524 else { //de x-waarde van de setpoint verandert niet
Gerber 22:02a9b5914cc7 525 }
Gerber 22:02a9b5914cc7 526
Gerber 22:02a9b5914cc7 527 if (Potmeterwaarde1>0.6) { //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
Gerber 22:02a9b5914cc7 528 Rsy += 0.001;
Gerber 22:02a9b5914cc7 529 }
Gerber 22:02a9b5914cc7 530 else if (Potmeterwaarde1<0.4) { //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
Gerber 22:02a9b5914cc7 531 Rsy -= 0.001;
Gerber 22:02a9b5914cc7 532 }
Gerber 22:02a9b5914cc7 533 else { //de y-waarde van de setpoint verandert niet
Gerber 22:02a9b5914cc7 534 }
Gerber 22:02a9b5914cc7 535 RKI();
Gerber 22:02a9b5914cc7 536 // hier the control of the 1st control system
Gerber 22:02a9b5914cc7 537 //double refP = GetReferencePosition(); //KOMT UIT RKI
Gerber 22:02a9b5914cc7 538 double Huidigepositie = motor1.getPosition();
Gerber 22:02a9b5914cc7 539 double error = (refP - Huidigepositie);// make an error
Gerber 22:02a9b5914cc7 540 double motorValue = FeedBackControl(error, e_prev, e_int);
Gerber 22:02a9b5914cc7 541 SetMotor1(motorValue);
Gerber 22:02a9b5914cc7 542 // hier the control of the 2nd control system
Gerber 22:02a9b5914cc7 543 //double refP2 = GetReferencePosition2();
Gerber 22:02a9b5914cc7 544 double Huidigepositie2 = motor2.getPosition();
Gerber 22:02a9b5914cc7 545 double error2 = (refP2 - Huidigepositie2);// make an error
Gerber 22:02a9b5914cc7 546 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Gerber 22:02a9b5914cc7 547 SetMotor2(motorValue2);
Gerber 22:02a9b5914cc7 548 pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
Gerber 22:02a9b5914cc7 549 }
Gerber 22:02a9b5914cc7 550
Gerber 22:02a9b5914cc7 551 void SetpointRobot()
Gerber 22:02a9b5914cc7 552 {
Gerber 22:02a9b5914cc7 553 if (automode == 0)
Gerber 22:02a9b5914cc7 554 {
Gerber 22:02a9b5914cc7 555 Treecko.detach();
Gerber 22:02a9b5914cc7 556 for (int n = 0; n < 100; n++)
Gerber 22:02a9b5914cc7 557 {
Gerber 22:02a9b5914cc7 558 Rsy -= 0.0002;
Gerber 22:02a9b5914cc7 559 RKI();
Gerber 22:02a9b5914cc7 560
Gerber 22:02a9b5914cc7 561 // hier the control of the control system
Gerber 22:02a9b5914cc7 562 double Huidigepositie = motor1.getPosition();
Gerber 22:02a9b5914cc7 563 double error = (refP - Huidigepositie);// make an error
Gerber 22:02a9b5914cc7 564 double motorValue = FeedBackControl(error, e_prev, e_int);
Gerber 22:02a9b5914cc7 565 SetMotor1(motorValue);
Gerber 22:02a9b5914cc7 566
Gerber 22:02a9b5914cc7 567 double Huidigepositie2 = motor2.getPosition();
Gerber 22:02a9b5914cc7 568 double error2 = (refP2 - Huidigepositie2);// make an error
Gerber 22:02a9b5914cc7 569 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Gerber 22:02a9b5914cc7 570 SetMotor2(motorValue2);
Gerber 22:02a9b5914cc7 571 //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
Gerber 22:02a9b5914cc7 572 pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
Gerber 22:02a9b5914cc7 573 }
Gerber 22:02a9b5914cc7 574
Gerber 22:02a9b5914cc7 575 for (int n = 0; n < 100; n++)
Gerber 22:02a9b5914cc7 576 {
Gerber 22:02a9b5914cc7 577 Rsy += 0.0002;
Gerber 22:02a9b5914cc7 578 RKI();
Gerber 22:02a9b5914cc7 579 // hier the control of the control system
Gerber 22:02a9b5914cc7 580 double Huidigepositie = motor1.getPosition();
Gerber 22:02a9b5914cc7 581 double error = (refP - Huidigepositie);// make an error
Gerber 22:02a9b5914cc7 582 double motorValue = FeedBackControl(error, e_prev, e_int);
Gerber 22:02a9b5914cc7 583 SetMotor1(motorValue);
Gerber 22:02a9b5914cc7 584
Gerber 22:02a9b5914cc7 585 double Huidigepositie2 = motor2.getPosition();
Gerber 22:02a9b5914cc7 586 double error2 = (refP2 - Huidigepositie2);// make an error
Gerber 22:02a9b5914cc7 587 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Gerber 22:02a9b5914cc7 588 SetMotor2(motorValue2);
Gerber 22:02a9b5914cc7 589 //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
Gerber 22:02a9b5914cc7 590 pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
Gerber 22:02a9b5914cc7 591 }
Gerber 22:02a9b5914cc7 592
Gerber 22:02a9b5914cc7 593 for (int n = 0; n < 100; n++)
Gerber 22:02a9b5914cc7 594 {
Gerber 22:02a9b5914cc7 595 Rsx += 0.0002;
Gerber 22:02a9b5914cc7 596 RKI();
Gerber 22:02a9b5914cc7 597 // hier the control of the control system
Gerber 22:02a9b5914cc7 598 double Huidigepositie = motor1.getPosition();
Gerber 22:02a9b5914cc7 599 double error = (refP - Huidigepositie);// make an error
Gerber 22:02a9b5914cc7 600 double motorValue = FeedBackControl(error, e_prev, e_int);
Gerber 22:02a9b5914cc7 601 SetMotor1(motorValue);
Gerber 22:02a9b5914cc7 602
Gerber 22:02a9b5914cc7 603 double Huidigepositie2 = motor2.getPosition();
Gerber 22:02a9b5914cc7 604 double error2 = (refP2 - Huidigepositie2);// make an error
Gerber 22:02a9b5914cc7 605 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Gerber 22:02a9b5914cc7 606 SetMotor2(motorValue2);
Gerber 22:02a9b5914cc7 607 //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
Gerber 22:02a9b5914cc7 608 pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
Gerber 22:02a9b5914cc7 609 }
Gerber 22:02a9b5914cc7 610
Gerber 22:02a9b5914cc7 611 for (int n = 0; n < 100; n++)
Gerber 22:02a9b5914cc7 612 {
Gerber 22:02a9b5914cc7 613 Rsx -= 0.0002;
Gerber 22:02a9b5914cc7 614 RKI();
Gerber 22:02a9b5914cc7 615 // hier the control of the control system
Gerber 22:02a9b5914cc7 616 double Huidigepositie = motor1.getPosition();
Gerber 22:02a9b5914cc7 617 double error = (refP - Huidigepositie);// make an error
Gerber 22:02a9b5914cc7 618 double motorValue = FeedBackControl(error, e_prev, e_int);
Gerber 22:02a9b5914cc7 619 SetMotor1(motorValue);
Gerber 22:02a9b5914cc7 620
Gerber 22:02a9b5914cc7 621 double Huidigepositie2 = motor2.getPosition();
Gerber 22:02a9b5914cc7 622 double error2 = (refP2 - Huidigepositie2);// make an error
Gerber 22:02a9b5914cc7 623 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Gerber 22:02a9b5914cc7 624 SetMotor2(motorValue2);
Gerber 22:02a9b5914cc7 625 //pc.printf("Motor2.getposition() 2 = %f\r\n",Huidigepositie2);
Gerber 22:02a9b5914cc7 626 pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
Gerber 22:02a9b5914cc7 627 }
Gerber 22:02a9b5914cc7 628
Gerber 22:02a9b5914cc7 629 automode = 1;
Gerber 22:02a9b5914cc7 630 }
Gerber 22:02a9b5914cc7 631
Gerber 22:02a9b5914cc7 632 else
Gerber 22:02a9b5914cc7 633 {
Gerber 22:02a9b5914cc7 634 Treecko.attach(DemoPotmeter,Ts);
Gerber 22:02a9b5914cc7 635 }
Gerber 22:02a9b5914cc7 636
Gerber 22:02a9b5914cc7 637 }
Gerber 22:02a9b5914cc7 638
Gerber 22:02a9b5914cc7 639 /*void SetpointRobot()
Gerber 22:02a9b5914cc7 640 {
Gerber 22:02a9b5914cc7 641 double Potmeterwaarde2 = potMeter2.read();
Gerber 22:02a9b5914cc7 642 double Potmeterwaarde1 = potMeter1.read();
Gerber 22:02a9b5914cc7 643
Gerber 22:02a9b5914cc7 644 if (Potmeterwaarde2>0.6) {
Gerber 22:02a9b5914cc7 645 Rsx += 0.001; //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
Gerber 22:02a9b5914cc7 646 }
Gerber 22:02a9b5914cc7 647 else if (Potmeterwaarde2<0.4) {
Gerber 22:02a9b5914cc7 648 Rsx -= 0.001; //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
Gerber 22:02a9b5914cc7 649 }
Gerber 22:02a9b5914cc7 650 else { //de x-waarde van de setpoint verandert niet
Gerber 22:02a9b5914cc7 651 }
Gerber 22:02a9b5914cc7 652
Gerber 22:02a9b5914cc7 653 if (Potmeterwaarde1>0.6) { //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
Gerber 22:02a9b5914cc7 654 Rsy += 0.001;
Gerber 22:02a9b5914cc7 655 }
Gerber 22:02a9b5914cc7 656 else if (Potmeterwaarde1<0.4) { //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
Gerber 22:02a9b5914cc7 657 Rsy -= 0.001;
Gerber 22:02a9b5914cc7 658 }
Gerber 22:02a9b5914cc7 659 else { //de y-waarde van de setpoint verandert niet
Gerber 22:02a9b5914cc7 660 }
Gerber 22:02a9b5914cc7 661 } */
Gerber 22:02a9b5914cc7 662
Gerber 22:02a9b5914cc7 663 double GetReferencePosition()
Gerber 22:02a9b5914cc7 664 {
Gerber 22:02a9b5914cc7 665 double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
Gerber 22:02a9b5914cc7 666 int maxwaarde = 4096; // = 64x64
Gerber 22:02a9b5914cc7 667 double refP = Potmeterwaarde*maxwaarde;
Gerber 22:02a9b5914cc7 668 return refP; // value between 0 and 4096
Gerber 22:02a9b5914cc7 669 }
Gerber 22:02a9b5914cc7 670
Gerber 22:02a9b5914cc7 671 double GetReferencePosition2()
Gerber 22:02a9b5914cc7 672 {
Gerber 22:02a9b5914cc7 673 double potmeterwaarde2 = potMeter1.read();
Gerber 22:02a9b5914cc7 674 int maxwaarde2 = 4096; // = 64x64
Gerber 22:02a9b5914cc7 675 double refP2 = potmeterwaarde2*maxwaarde2;
Gerber 22:02a9b5914cc7 676 return refP2; // value between 0 and 4096
Gerber 22:02a9b5914cc7 677 }
Gerber 22:02a9b5914cc7 678
Gerber 22:02a9b5914cc7 679
Gerber 22:02a9b5914cc7 680
paulineoonk 16:2f89d6e25782 681 void MeasureAndControl(void)
paulineoonk 16:2f89d6e25782 682 {
Gerber 21:9307dc9be4f7 683 //SetpointRobot();
Gerber 21:9307dc9be4f7 684 changePosition();
Gerber 18:1e4f697a92cb 685 // RKI aanroepen
Gerber 18:1e4f697a92cb 686 RKI();
paulineoonk 16:2f89d6e25782 687 // hier the control of the 1st control system
Gerber 18:1e4f697a92cb 688 //double refP = GetReferencePosition(); //KOMT UIT RKI
paulineoonk 16:2f89d6e25782 689 double Huidigepositie = motor1.getPosition();
paulineoonk 16:2f89d6e25782 690 double error = (refP - Huidigepositie);// make an error
paulineoonk 16:2f89d6e25782 691 double motorValue = FeedBackControl(error, e_prev, e_int);
paulineoonk 16:2f89d6e25782 692 SetMotor1(motorValue);
paulineoonk 16:2f89d6e25782 693 // hier the control of the 2nd control system
Gerber 18:1e4f697a92cb 694 //double refP2 = GetReferencePosition2();
paulineoonk 16:2f89d6e25782 695 double Huidigepositie2 = motor2.getPosition();
paulineoonk 16:2f89d6e25782 696 double error2 = (refP2 - Huidigepositie2);// make an error
paulineoonk 16:2f89d6e25782 697 double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
paulineoonk 16:2f89d6e25782 698 SetMotor2(motorValue2);
Gerber 20:14edaecd7413 699 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 700 }
paulineoonk 16:2f89d6e25782 701
Gerber 21:9307dc9be4f7 702 /*void changePosition () // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
paulineoonk 11:b46a4c48c08f 703 {
paulineoonk 11:b46a4c48c08f 704 if (RBF>0.3) {
paulineoonk 11:b46a4c48c08f 705 goalx++; // hoe veel verder gaat hij? 1 cm? 10 cm?
paulineoonk 11:b46a4c48c08f 706 }
paulineoonk 11:b46a4c48c08f 707 if (RTF>0.3) {
paulineoonk 11:b46a4c48c08f 708 goalx--;
paulineoonk 11:b46a4c48c08f 709 }
paulineoonk 11:b46a4c48c08f 710 if (LBF>0.3) {
paulineoonk 11:b46a4c48c08f 711 goaly++;
paulineoonk 11:b46a4c48c08f 712 }
paulineoonk 11:b46a4c48c08f 713 if (LTF>0.3) {
paulineoonk 11:b46a4c48c08f 714 goaly--;
paulineoonk 11:b46a4c48c08f 715 }
paulineoonk 12:65b8d29bdd5d 716 pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
paulineoonk 11:b46a4c48c08f 717 }
Gerber 21:9307dc9be4f7 718 */
paulineoonk 14:a861ba49107c 719 void Loop_funtion()
paulineoonk 14:a861ba49107c 720 {
Gerber 20:14edaecd7413 721 pc.printf("state machine begint \r\n");
paulineoonk 14:a861ba49107c 722 switch(State){
paulineoonk 14:a861ba49107c 723 case Cal1: //Calibration motor 1
Gerber 20:14edaecd7413 724 pc.printf("cal1 \r\n");
Gerber 19:591572f4e4b5 725 State=Cal2;
paulineoonk 14:a861ba49107c 726 // naar achteren bewegen( als voorbeeld Arvid), daarna deze waarde opslaan als offset. Dan bewegen naar home middels PID en verschil encodervalue uiterste stand en home1.
Gerber 19:591572f4e4b5 727 /* motorValue1 = 0.1f; motorValue2=0;
paulineoonk 16:2f89d6e25782 728 M2E = fabs(motorValue2);
paulineoonk 16:2f89d6e25782 729 M1E = fabs(motorValue1);
Gerber 19:591572f4e4b5 730
paulineoonk 14:a861ba49107c 731 if (Huidigepositie1== 0)
paulineoonk 14:a861ba49107c 732 {
paulineoonk 16:2f89d6e25782 733 SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,.
Gerber 19:591572f4e4b5 734 //if (fabs(Huidigepositie1<0.01) {
paulineoonk 15:1cfe58aea10d 735 State=Cal2;
Gerber 19:591572f4e4b5 736 //}
paulineoonk 14:a861ba49107c 737 }
paulineoonk 14:a861ba49107c 738 else {
paulineoonk 14:a861ba49107c 739 SetMotor1(0);
paulineoonk 15:1cfe58aea10d 740 Loop_funtion();
Gerber 19:591572f4e4b5 741 }*/
paulineoonk 14:a861ba49107c 742 break;
paulineoonk 14:a861ba49107c 743
paulineoonk 14:a861ba49107c 744 case Cal2: //Calibration motor 2
Gerber 20:14edaecd7413 745 State = CalEMG;
Gerber 19:591572f4e4b5 746 /* if (Huidigepositie2== 0)
paulineoonk 14:a861ba49107c 747 {
paulineoonk 15:1cfe58aea10d 748 if (Huidigepositie2<0.01){
paulineoonk 15:1cfe58aea10d 749 State=CalEMG;
paulineoonk 14:a861ba49107c 750 }
paulineoonk 14:a861ba49107c 751 else {
paulineoonk 14:a861ba49107c 752 SetMotor2(0);
paulineoonk 15:1cfe58aea10d 753 Loop_funtion();
Gerber 19:591572f4e4b5 754 } */
paulineoonk 14:a861ba49107c 755 break;
paulineoonk 14:a861ba49107c 756 case CalEMG: // Calibration EMG
paulineoonk 15:1cfe58aea10d 757 CalibrationEMG(); //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
paulineoonk 14:a861ba49107c 758 break;
paulineoonk 14:a861ba49107c 759 case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
Gerber 22:02a9b5914cc7 760 wait(2.0);
Gerber 22:02a9b5914cc7 761 pc.printf("besturingsmethode kiezen \r\n");
paulineoonk 15:1cfe58aea10d 762 if (button==1) {
paulineoonk 15:1cfe58aea10d 763 State=EMG;
paulineoonk 14:a861ba49107c 764 }
paulineoonk 15:1cfe58aea10d 765 if (button==0) {
paulineoonk 15:1cfe58aea10d 766 State=Demo;
paulineoonk 14:a861ba49107c 767 }
paulineoonk 14:a861ba49107c 768 break;
paulineoonk 14:a861ba49107c 769 case EMG: //Aansturen met EMG
Gerber 20:14edaecd7413 770 pc.printf("EMG begint met aansturen \r\n");
paulineoonk 14:a861ba49107c 771 Filteren();
Gerber 21:9307dc9be4f7 772 //changePosition();
paulineoonk 16:2f89d6e25782 773 //RKI --> output refP van motor
paulineoonk 16:2f89d6e25782 774 MeasureAndControl();
paulineoonk 14:a861ba49107c 775 break;
paulineoonk 15:1cfe58aea10d 776 case Demo: // Aansturen met toetsenbord
Gerber 22:02a9b5914cc7 777 pc.printf("Automatische DEMO \r\n");
Gerber 22:02a9b5914cc7 778 SetpointRobot();
Gerber 22:02a9b5914cc7 779 State=STOP;
Gerber 22:02a9b5914cc7 780 break;
Gerber 22:02a9b5914cc7 781 case STOP: // Demo runt zichtzelf
Gerber 22:02a9b5914cc7 782 SetMotor1(0);
Gerber 22:02a9b5914cc7 783 SetMotor2(0);
paulineoonk 14:a861ba49107c 784 break;
paulineoonk 15:1cfe58aea10d 785 }
paulineoonk 14:a861ba49107c 786 }
Miriam 0:d5fb29bc0847 787
paulineoonk 15:1cfe58aea10d 788 /*void Tickerfunctie()
paulineoonk 8:c4ec359af35d 789 {
paulineoonk 13:3351f4374885 790 //if(caldone == false)
paulineoonk 13:3351f4374885 791 //{
paulineoonk 12:65b8d29bdd5d 792 pc.printf("emgreadRB = %f , emgFiltered = %f, maxi = %f meanrest = %f\r\n",emgRB.read(), RBF, MVCRB, RESTMEANLB);
paulineoonk 11:b46a4c48c08f 793 pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, meanrest = %f emgSUMLB %f ,Timescalibration %i\r\n",emgLB.read(), LBF, MVCLB,RESTMEANRB,emgSUMLB, Timescalibration);
paulineoonk 10:518a8617c86e 794 pc.printf("emgreadRT = %f , emgFilteredRT = %f, maxiRT = %f meanrest = %f \r\n",emgRT.read(), RTF, MVCRT,RESTMEANRT);
paulineoonk 10:518a8617c86e 795 pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f meanrest = %f \r\n",emgLT.read(), LTF, MVCLT,RESTMEANLT);
paulineoonk 13:3351f4374885 796 //}
paulineoonk 8:c4ec359af35d 797 }
paulineoonk 15:1cfe58aea10d 798 */
Gerber 19:591572f4e4b5 799 int main()//deze moet ooit nog weg --> pas op voor errors
Miriam 0:d5fb29bc0847 800 {
paulineoonk 7:05c71a859d27 801 //voor EMG filteren
paulineoonk 10:518a8617c86e 802 //Left Bicep
paulineoonk 9:285499f48cdd 803 NFLB.add( &N1LB );
paulineoonk 10:518a8617c86e 804 HPFLB.add( &HP1LB ).add( &HP2LB );
paulineoonk 9:285499f48cdd 805 LPFLB.add( &LP1LB ).add( &LP2LB );
paulineoonk 9:285499f48cdd 806
paulineoonk 10:518a8617c86e 807 //Right Bicep
paulineoonk 10:518a8617c86e 808 NFRB.add( &N1RB );
paulineoonk 10:518a8617c86e 809 HPFRB.add( &HP1RB ).add( &HP2RB );
paulineoonk 10:518a8617c86e 810 LPFRB.add( &LP1RB ).add( &LP2RB );
paulineoonk 9:285499f48cdd 811
paulineoonk 10:518a8617c86e 812 //Left Tricep
paulineoonk 10:518a8617c86e 813 NFLT.add( &N1LT );
paulineoonk 10:518a8617c86e 814 HPFLT.add( &HP1LT ).add( &HP2LT );
paulineoonk 10:518a8617c86e 815 LPFLT.add( &LP1LT ).add( &LP2LT );
paulineoonk 10:518a8617c86e 816
paulineoonk 10:518a8617c86e 817 //Right Tricep
paulineoonk 10:518a8617c86e 818 NFRT.add( &N1RT );
paulineoonk 10:518a8617c86e 819 HPFRT.add( &HP1RT ).add( &HP2RT );
paulineoonk 10:518a8617c86e 820 LPFRT.add( &LP1RT ).add( &LP2RT );
paulineoonk 3:36e706d6b3d2 821
paulineoonk 7:05c71a859d27 822 //voor serial
paulineoonk 7:05c71a859d27 823 pc.baud(115200);
Gerber 20:14edaecd7413 824 pc.printf("begint met programma \r\n");
paulineoonk 7:05c71a859d27 825
paulineoonk 7:05c71a859d27 826 //motor
Gerber 20:14edaecd7413 827 M1E.period(PwmPeriod); //set PWMposition at 5000hz
paulineoonk 7:05c71a859d27 828 //Ticker
paulineoonk 15:1cfe58aea10d 829 //Treecko.attach(MeasureAndControl, tijdstap); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
paulineoonk 3:36e706d6b3d2 830 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
paulineoonk 15:1cfe58aea10d 831 // printer.attach(Tickerfunctie,0.4);
paulineoonk 14:a861ba49107c 832
paulineoonk 14:a861ba49107c 833 //State Machine
paulineoonk 14:a861ba49107c 834 State = Cal1;
paulineoonk 14:a861ba49107c 835 Position_controller_on = false;
Gerber 20:14edaecd7413 836 Treecko.attach(&Loop_funtion, Ts);
paulineoonk 3:36e706d6b3d2 837 while(true)
paulineoonk 16:2f89d6e25782 838 { }
paulineoonk 3:36e706d6b3d2 839
paulineoonk 16:2f89d6e25782 840 //is deze wel nodig?
paulineoonk 16:2f89d6e25782 841 }