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@7:05c71a859d27, 2017-10-31 (annotated)

Committer:
paulineoonk
Date:
Tue Oct 31 15:42:05 2017 +0000
Revision:
7:05c71a859d27
Parent:
6:e0e5da2c068f
Child:
8:c4ec359af35d
EMG met calibratie erbij

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