Projectgroep 20 Biorobotics / Mbed 2 deprecated vanEMGnaarMOTORPauline_States_nacht

State Machine, troep nog niet verwijderd.

Dependencies:   Encoder HIDScope MODSERIAL biquadFilter mbed

Fork of vanEMGnaarMOTORPauline by Projectgroep 20 Biorobotics

main.cpp@8:c4ec359af35d, 2017-10-31 (annotated)

Committer:
paulineoonk
Date:
Tue Oct 31 20:43:42 2017 +0000
Revision:
8:c4ec359af35d
Parent:
7:05c71a859d27
Child:
9:285499f48cdd
Avondwerk - 4 emg - nog testen

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 3:36e706d6b3d2 8 //globalvariables Motor
paulineoonk 3:36e706d6b3d2 9 Ticker Treecko; //We make a awesome ticker for our control system
paulineoonk 8:c4ec359af35d 10 Ticker printer;
paulineoonk 8:c4ec359af35d 11 //PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed
paulineoonk 8:c4ec359af35d 12 //DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control
Miriam 0:d5fb29bc0847 13
paulineoonk 8:c4ec359af35d 14 //Encoder motor1(D13,D12,true);
paulineoonk 3:36e706d6b3d2 15 MODSERIAL pc(USBTX,USBRX);
paulineoonk 3:36e706d6b3d2 16
paulineoonk 8:c4ec359af35d 17 //double PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
paulineoonk 3:36e706d6b3d2 18 const double Ts = 0.1; // tickettijd/ sample time
paulineoonk 8:c4ec359af35d 19 //double e_prev = 0;
paulineoonk 8:c4ec359af35d 20 //double e_int = 0;
paulineoonk 6:e0e5da2c068f 21 double tijdstap = 0.002;
paulineoonk 8:c4ec359af35d 22 volatile double LBF;
paulineoonk 8:c4ec359af35d 23 volatile double RBF;
paulineoonk 8:c4ec359af35d 24 volatile double LTF;
paulineoonk 8:c4ec359af35d 25 volatile double RTF;
Miriam 0:d5fb29bc0847 26
paulineoonk 7:05c71a859d27 27 //buttons en leds voor calibration
paulineoonk 7:05c71a859d27 28 DigitalIn button1(PTA4);
paulineoonk 8:c4ec359af35d 29 DigitalOut ledred(LED_RED);
paulineoonk 7:05c71a859d27 30 DigitalOut ledblue(LED_BLUE);
paulineoonk 8:c4ec359af35d 31
paulineoonk 8:c4ec359af35d 32 //double maxiLB = 0;
paulineoonk 8:c4ec359af35d 33 //double maxiRB = 0;
paulineoonk 8:c4ec359af35d 34 //double maxiLT = 0;
paulineoonk 8:c4ec359af35d 35 //double maxiRT = 0;
paulineoonk 8:c4ec359af35d 36
paulineoonk 7:05c71a859d27 37 bool caldone = false;
paulineoonk 8:c4ec359af35d 38 int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
paulineoonk 8:c4ec359af35d 39
paulineoonk 8:c4ec359af35d 40 int TimescalibrationLB = 0;
paulineoonk 8:c4ec359af35d 41 int TimescalibrationRB = 0;
paulineoonk 8:c4ec359af35d 42 int TimescalibrationLT = 0;
paulineoonk 8:c4ec359af35d 43 int TimescalibrationRT = 0;
Miriam 0:d5fb29bc0847 44
Miriam 0:d5fb29bc0847 45 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
Miriam 0:d5fb29bc0847 46 BiQuad N1( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
Miriam 0:d5fb29bc0847 47 BiQuadChain NF;
Miriam 0:d5fb29bc0847 48 BiQuad HP1( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
Miriam 0:d5fb29bc0847 49 BiQuad HP2( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
Miriam 0:d5fb29bc0847 50 BiQuadChain HPF;
Miriam 0:d5fb29bc0847 51 BiQuad LP1( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
Miriam 0:d5fb29bc0847 52 BiQuad LP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
Miriam 0:d5fb29bc0847 53 BiQuadChain LPF;
paulineoonk 7:05c71a859d27 54
paulineoonk 7:05c71a859d27 55 Timer looptime; //moetuiteindelijk weg
paulineoonk 6:e0e5da2c068f 56
paulineoonk 7:05c71a859d27 57 //filters
paulineoonk 8:c4ec359af35d 58 double emgNotchLB;
paulineoonk 8:c4ec359af35d 59 double emgHPLB;
paulineoonk 8:c4ec359af35d 60 double emgAbsHPLB;
paulineoonk 8:c4ec359af35d 61 double emgLPLB;
paulineoonk 8:c4ec359af35d 62
paulineoonk 8:c4ec359af35d 63 double emgNotchRB;
paulineoonk 8:c4ec359af35d 64 double emgHPRB;
paulineoonk 8:c4ec359af35d 65 double emgAbsHPRB;
paulineoonk 8:c4ec359af35d 66 double emgLPRB;
paulineoonk 7:05c71a859d27 67
paulineoonk 8:c4ec359af35d 68 double emgNotchLT;
paulineoonk 8:c4ec359af35d 69 double emgHPLT;
paulineoonk 8:c4ec359af35d 70 double emgAbsHPLT;
paulineoonk 8:c4ec359af35d 71 double emgLPLT;
paulineoonk 7:05c71a859d27 72
paulineoonk 8:c4ec359af35d 73 double emgNotchRT;
paulineoonk 8:c4ec359af35d 74 double emgHPRT;
paulineoonk 8:c4ec359af35d 75 double emgAbsHPRT;
paulineoonk 8:c4ec359af35d 76 double emgLPRT;
Miriam 0:d5fb29bc0847 77
paulineoonk 3:36e706d6b3d2 78 double f = 500; // frequency
paulineoonk 3:36e706d6b3d2 79 double dt = 1/f; // sample frequency
paulineoonk 8:c4ec359af35d 80
paulineoonk 8:c4ec359af35d 81 AnalogIn emgLB(A0); // EMG lezen
paulineoonk 8:c4ec359af35d 82 AnalogIn emgRB(A1);
paulineoonk 8:c4ec359af35d 83 AnalogIn emgLT(A2);
paulineoonk 8:c4ec359af35d 84 AnalogIn emgRT(A3);
Miriam 0:d5fb29bc0847 85
paulineoonk 8:c4ec359af35d 86 float maxiLB = 0.3;
paulineoonk 8:c4ec359af35d 87 float maxiRB = 0.3;
paulineoonk 8:c4ec359af35d 88 float maxiLT = 0.3;
paulineoonk 8:c4ec359af35d 89 float maxiRT = 0.3;
paulineoonk 8:c4ec359af35d 90
paulineoonk 8:c4ec359af35d 91 void Filteren()
Miriam 0:d5fb29bc0847 92 {
paulineoonk 6:e0e5da2c068f 93 looptime.reset();
paulineoonk 6:e0e5da2c068f 94 looptime.start();
paulineoonk 8:c4ec359af35d 95
paulineoonk 8:c4ec359af35d 96 //EMG 1
paulineoonk 8:c4ec359af35d 97
paulineoonk 8:c4ec359af35d 98 emgNotchLB = NF.step(emgLB.read() ); // Notch filter
paulineoonk 8:c4ec359af35d 99 emgHPLB = HPF.step(emgNotchLB); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 100 emgAbsHPLB = abs(emgHPLB); // Take absolute value
paulineoonk 8:c4ec359af35d 101 emgLPLB = LPF.step(emgAbsHPLB); // Low-pass filter: creates envelope
paulineoonk 8:c4ec359af35d 102 LBF = emgLPLB/maxiLB; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 103 /*
paulineoonk 8:c4ec359af35d 104 emgNotchRB = NF.step(emgRB.read() ); // Notch filter
paulineoonk 8:c4ec359af35d 105 emgHPRB = HPF.step(emgNotchRB); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 106 emgAbsHPRB = abs(emgHPRB); // Take absolute value
paulineoonk 8:c4ec359af35d 107 emgLPRB = LPF.step(emgAbsHPRB); // Low-pass filter: creates envelope
paulineoonk 8:c4ec359af35d 108 RBF = emgLPRB/maxiRB; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 109
paulineoonk 8:c4ec359af35d 110 emgNotchLT = NF.step(emgLT.read() ); // Notch filter
paulineoonk 8:c4ec359af35d 111 emgHPLT = HPF.step(emgNotchLT); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 112 emgAbsHPLT = abs(emgHPLT); // Take absolute value
paulineoonk 8:c4ec359af35d 113 emgLPLT = LPF.step(emgAbsHPLT); // Low-pass filter: creates envelope
paulineoonk 8:c4ec359af35d 114 LTF = emgLPLT/maxiLT; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 115
paulineoonk 8:c4ec359af35d 116 emgNotchRT = NF.step(emgRT.read() ); // Notch filter
paulineoonk 8:c4ec359af35d 117 emgHPRT = HPF.step(emgNotchRT); // High-pass filter: also normalises around 0.
paulineoonk 8:c4ec359af35d 118 emgAbsHPRT = abs(emgHPRT); // Take absolute value
paulineoonk 8:c4ec359af35d 119 emgLPRT = LPF.step(emgAbsHPRT); // Low-pass filter: creates envelope
paulineoonk 8:c4ec359af35d 120 RTF = emgLPRT/maxiRT; // Scale to maximum signal: useful for motor
paulineoonk 8:c4ec359af35d 121
paulineoonk 8:c4ec359af35d 122 //if (emgFiltered >1)
paulineoonk 8:c4ec359af35d 123 //{
paulineoonk 8:c4ec359af35d 124 // emgFiltered=1.00;
paulineoonk 8:c4ec359af35d 125 //}
paulineoonk 8:c4ec359af35d 126 //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 127 //int maxwaarde = 4096; // = 64x64
paulineoonk 6:e0e5da2c068f 128 //double refP = emgFiltered*maxwaarde;
paulineoonk 6:e0e5da2c068f 129 //return refP; // value between 0 and 4096
paulineoonk 8:c4ec359af35d 130 */
Miriam 0:d5fb29bc0847 131 }
paulineoonk 8:c4ec359af35d 132 /*
paulineoonk 7:05c71a859d27 133 void Calibration()
paulineoonk 7:05c71a859d27 134 {
paulineoonk 7:05c71a859d27 135 Timescalibration++;
paulineoonk 7:05c71a859d27 136 emgNotch = NF.step(emg.read()); // Notch filter
paulineoonk 7:05c71a859d27 137 emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0.
paulineoonk 7:05c71a859d27 138 emgAbsHP = abs(emgHP); // Take absolute value
paulineoonk 7:05c71a859d27 139 emgLP = LPF.step(emgAbsHP); // Low-pass filter: creates envelope
paulineoonk 7:05c71a859d27 140 double emgfinal = emgLP;
paulineoonk 7:05c71a859d27 141 if (emgfinal > maxi)
paulineoonk 7:05c71a859d27 142 { //determine what the highest reachable emg signal is
paulineoonk 7:05c71a859d27 143 maxi = emgfinal;
paulineoonk 7:05c71a859d27 144 }
paulineoonk 7:05c71a859d27 145 if(Timescalibration>5000)
paulineoonk 7:05c71a859d27 146 {
paulineoonk 7:05c71a859d27 147 caldone=true;
paulineoonk 7:05c71a859d27 148 }
paulineoonk 7:05c71a859d27 149 // pc.printf("caldone = %i , Timescalibration = %i maxi = %f \r\n",caldone,Timescalibration,maxi);
paulineoonk 7:05c71a859d27 150
paulineoonk 7:05c71a859d27 151 //for(int n =0; n<CalibrationSample;n++) //read for 5000 samples as calibration
paulineoonk 7:05c71a859d27 152 //{
paulineoonk 7:05c71a859d27 153 // emgNotch = NF.step(emg.read()); // Notch filter
paulineoonk 7:05c71a859d27 154 // emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0.
paulineoonk 7:05c71a859d27 155 //emgAbsHP = abs(emgHP); // Take absolute value
paulineoonk 7:05c71a859d27 156 //emgLP = LPF.step(emgAbsHP); // Low-pass filter: creates envelope
paulineoonk 7:05c71a859d27 157 //double emgfinal = emgLP;
paulineoonk 7:05c71a859d27 158 //if (emgfinal > maxi)
paulineoonk 7:05c71a859d27 159 // { //determine what the highest reachable emg signal is
paulineoonk 7:05c71a859d27 160 // maxi = emgfinal;
paulineoonk 7:05c71a859d27 161 //}
paulineoonk 7:05c71a859d27 162 // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
paulineoonk 7:05c71a859d27 163 //}
paulineoonk 7:05c71a859d27 164 //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
paulineoonk 7:05c71a859d27 165 //return maxi;
paulineoonk 7:05c71a859d27 166 }
paulineoonk 7:05c71a859d27 167
paulineoonk 8:c4ec359af35d 168 */
paulineoonk 7:05c71a859d27 169
paulineoonk 6:e0e5da2c068f 170 /*
paulineoonk 3:36e706d6b3d2 171 double Encoder ()
paulineoonk 3:36e706d6b3d2 172 {
paulineoonk 3:36e706d6b3d2 173 double Huidigepositie = motor1.getPosition ();
paulineoonk 3:36e706d6b3d2 174 return Huidigepositie; // huidige positie = current position
paulineoonk 3:36e706d6b3d2 175 }
paulineoonk 6:e0e5da2c068f 176
paulineoonk 3:36e706d6b3d2 177 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)
Miriam 0:d5fb29bc0847 178 {
paulineoonk 3:36e706d6b3d2 179 double kp = 0.001; // has jet to be scaled
paulineoonk 3:36e706d6b3d2 180 double Proportional= kp*error;
Miriam 0:d5fb29bc0847 181
paulineoonk 3:36e706d6b3d2 182 double kd = 0.0004; // has jet to be scaled
paulineoonk 3:36e706d6b3d2 183 double VelocityError = (error - e_prev)/Ts;
paulineoonk 3:36e706d6b3d2 184 double Derivative = kd*VelocityError;
Miriam 0:d5fb29bc0847 185 e_prev = error;
Miriam 0:d5fb29bc0847 186
paulineoonk 3:36e706d6b3d2 187 double ki = 0.00005; // has jet to be scaled
Miriam 0:d5fb29bc0847 188 e_int = e_int+Ts*error;
paulineoonk 3:36e706d6b3d2 189 double Integrator = ki*e_int;
Miriam 0:d5fb29bc0847 190
Miriam 0:d5fb29bc0847 191
paulineoonk 3:36e706d6b3d2 192 double motorValue = Proportional + Integrator + Derivative;
Miriam 0:d5fb29bc0847 193 return motorValue;
Miriam 0:d5fb29bc0847 194 }
paulineoonk 6:e0e5da2c068f 195
paulineoonk 3:36e706d6b3d2 196 void SetMotor1(double motorValue)
Miriam 0:d5fb29bc0847 197 {
Miriam 0:d5fb29bc0847 198 if (motorValue >= 0)
Miriam 0:d5fb29bc0847 199 {
Miriam 0:d5fb29bc0847 200 M1D = 0;
Miriam 0:d5fb29bc0847 201 }
Miriam 0:d5fb29bc0847 202 else
Miriam 0:d5fb29bc0847 203 {
Miriam 0:d5fb29bc0847 204 M1D = 1;
Miriam 0:d5fb29bc0847 205 }
Miriam 0:d5fb29bc0847 206
Miriam 0:d5fb29bc0847 207 if (fabs(motorValue) > 1)
Miriam 0:d5fb29bc0847 208 {
Miriam 0:d5fb29bc0847 209 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Miriam 0:d5fb29bc0847 210 }
Miriam 0:d5fb29bc0847 211 else
Miriam 0:d5fb29bc0847 212 {
Miriam 0:d5fb29bc0847 213 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Miriam 0:d5fb29bc0847 214 }
Miriam 0:d5fb29bc0847 215 }
paulineoonk 7:05c71a859d27 216 */
paulineoonk 3:36e706d6b3d2 217 void MeasureAndControl ()
Miriam 0:d5fb29bc0847 218 {
paulineoonk 3:36e706d6b3d2 219 // hier the control of the control system
paulineoonk 7:05c71a859d27 220
paulineoonk 8:c4ec359af35d 221 //if(caldone==false)
paulineoonk 8:c4ec359af35d 222 //{
paulineoonk 8:c4ec359af35d 223 // if(button1.read()==false)
paulineoonk 8:c4ec359af35d 224 // {
paulineoonk 8:c4ec359af35d 225 // Calibration();
paulineoonk 8:c4ec359af35d 226 // }
paulineoonk 8:c4ec359af35d 227 //}
paulineoonk 8:c4ec359af35d 228 //if (caldone==true)
paulineoonk 7:05c71a859d27 229
paulineoonk 8:c4ec359af35d 230 //{
paulineoonk 8:c4ec359af35d 231 Filteren();
paulineoonk 7:05c71a859d27 232 //rest
paulineoonk 8:c4ec359af35d 233 //}
paulineoonk 7:05c71a859d27 234
paulineoonk 4:5607088ef6f5 235 //double Huidigepositie = Encoder();
paulineoonk 4:5607088ef6f5 236 //double error = (refP - Huidigepositie);// make an error
paulineoonk 4:5607088ef6f5 237 //double motorValue = FeedBackControl(error, e_prev, e_int);
paulineoonk 7:05c71a859d27 238 //double motorValue = refP;
paulineoonk 7:05c71a859d27 239 //SetMotor1(motorValue);
Miriam 0:d5fb29bc0847 240 }
Miriam 0:d5fb29bc0847 241
paulineoonk 8:c4ec359af35d 242 void Tickerfunctie()
paulineoonk 8:c4ec359af35d 243 {
paulineoonk 8:c4ec359af35d 244 pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n",emgLB.read(), LBF, maxiLB,looptime.read());
paulineoonk 8:c4ec359af35d 245 //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 8:c4ec359af35d 246 }
paulineoonk 7:05c71a859d27 247
Miriam 0:d5fb29bc0847 248 int main()
Miriam 0:d5fb29bc0847 249 {
paulineoonk 7:05c71a859d27 250 //voor EMG filteren
Miriam 0:d5fb29bc0847 251 NF.add( &N1 );
Miriam 0:d5fb29bc0847 252 HPF.add( &HP1 ).add( &HP2 );
Miriam 0:d5fb29bc0847 253 LPF.add( &LP1 ).add( &LP2 );
paulineoonk 3:36e706d6b3d2 254
paulineoonk 7:05c71a859d27 255 //voor serial
paulineoonk 7:05c71a859d27 256 pc.baud(115200);
paulineoonk 7:05c71a859d27 257
paulineoonk 7:05c71a859d27 258 //motor
paulineoonk 8:c4ec359af35d 259 // M1E.period(PwmPeriod); //set PWMposition at 5000hz
paulineoonk 7:05c71a859d27 260 //Ticker
paulineoonk 7:05c71a859d27 261 Treecko.attach(MeasureAndControl, tijdstap); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
paulineoonk 3:36e706d6b3d2 262 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
paulineoonk 8:c4ec359af35d 263 printer.attach(Tickerfunctie,0.1);
paulineoonk 3:36e706d6b3d2 264 while(true)
Miriam 0:d5fb29bc0847 265 {
paulineoonk 3:36e706d6b3d2 266 }
paulineoonk 3:36e706d6b3d2 267
paulineoonk 3:36e706d6b3d2 268
paulineoonk 3:36e706d6b3d2 269 }