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@5:daa916945271, 2017-10-30 (annotated)

Committer:
paulineoonk
Date:
Mon Oct 30 13:33:32 2017 +0000
Revision:
5:daa916945271
Parent:
4:5607088ef6f5
Child:
6:e0e5da2c068f
Leest EMG goed uit in hidscope en terraterm. Tickersample = 0.1 s. Waarden uitlezen als floats;

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 3:36e706d6b3d2 20
paulineoonk 3:36e706d6b3d2 21 //globalvariables filter
Miriam 0:d5fb29bc0847 22
Miriam 0:d5fb29bc0847 23 //Hidscope aanmaken
Miriam 0:d5fb29bc0847 24 HIDScope scope(2);
paulineoonk 3:36e706d6b3d2 25 double maxi = 0.12; // max signal after filtering, 0.1-0.12
Miriam 0:d5fb29bc0847 26
Miriam 0:d5fb29bc0847 27 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
Miriam 0:d5fb29bc0847 28 BiQuad N1( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
Miriam 0:d5fb29bc0847 29 BiQuadChain NF;
Miriam 0:d5fb29bc0847 30 BiQuad HP1( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
Miriam 0:d5fb29bc0847 31 BiQuad HP2( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
Miriam 0:d5fb29bc0847 32 BiQuadChain HPF;
Miriam 0:d5fb29bc0847 33 BiQuad LP1( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
Miriam 0:d5fb29bc0847 34 BiQuad LP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
Miriam 0:d5fb29bc0847 35 BiQuadChain LPF;
Miriam 0:d5fb29bc0847 36
paulineoonk 3:36e706d6b3d2 37 double f = 500; // frequency
paulineoonk 3:36e706d6b3d2 38 double dt = 1/f; // sample frequency
Miriam 0:d5fb29bc0847 39 AnalogIn emg(A0); // EMG lezen
Miriam 0:d5fb29bc0847 40
paulineoonk 3:36e706d6b3d2 41 double GetReferencePosition()
Miriam 0:d5fb29bc0847 42 {
paulineoonk 4:5607088ef6f5 43
Miriam 0:d5fb29bc0847 44 double emgNotch = NF.step(emg.read() ); // Notch filter
Miriam 0:d5fb29bc0847 45 double emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0.
Miriam 0:d5fb29bc0847 46 double emgAbsHP = abs(emgHP); // Take absolute value
Miriam 0:d5fb29bc0847 47 double emgLP = LPF.step(emgAbsHP); // Low-pass filter: creates envelope
Miriam 0:d5fb29bc0847 48 double emgMax = maxi; //(emgLP); // moet waarde 'schatten' voor de max, want je leest de data live. voorbeeld: 0.1, maar mogelijk 0.2 kiezen voor veiligheidsfactor. Dan gaat motor alleen maximaal op 1/2 vermogen.
Miriam 0:d5fb29bc0847 49 double emgFiltered = emgLP/emgMax; // Scale to maximum signal: useful for motor
Miriam 0:d5fb29bc0847 50 if (emgFiltered >1)
Miriam 0:d5fb29bc0847 51 {
Miriam 0:d5fb29bc0847 52 emgFiltered=1.00;
Miriam 0:d5fb29bc0847 53 }
Miriam 0:d5fb29bc0847 54 scope.set(0,emgFiltered);
Miriam 0:d5fb29bc0847 55 scope.set(1,emg.read());
Miriam 0:d5fb29bc0847 56 scope.send();
paulineoonk 4:5607088ef6f5 57 pc.baud(115200);
paulineoonk 5:daa916945271 58 printf("emgread = %f , emgFiltered = %f \r\n",emg.read(), emgFiltered);
paulineoonk 3:36e706d6b3d2 59 int maxwaarde = 4096; // = 64x64
paulineoonk 3:36e706d6b3d2 60 double refP = emgFiltered*maxwaarde;
Miriam 2:bc01b1ce8fb6 61 return refP; // value between 0 and 4096
Miriam 0:d5fb29bc0847 62 }
paulineoonk 4:5607088ef6f5 63
paulineoonk 3:36e706d6b3d2 64 double Encoder ()
paulineoonk 3:36e706d6b3d2 65 {
paulineoonk 3:36e706d6b3d2 66 double Huidigepositie = motor1.getPosition ();
paulineoonk 3:36e706d6b3d2 67 return Huidigepositie; // huidige positie = current position
paulineoonk 3:36e706d6b3d2 68 }
paulineoonk 4:5607088ef6f5 69 /*
paulineoonk 3:36e706d6b3d2 70 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 71 {
paulineoonk 3:36e706d6b3d2 72 double kp = 0.001; // has jet to be scaled
paulineoonk 3:36e706d6b3d2 73 double Proportional= kp*error;
Miriam 0:d5fb29bc0847 74
paulineoonk 3:36e706d6b3d2 75 double kd = 0.0004; // has jet to be scaled
paulineoonk 3:36e706d6b3d2 76 double VelocityError = (error - e_prev)/Ts;
paulineoonk 3:36e706d6b3d2 77 double Derivative = kd*VelocityError;
Miriam 0:d5fb29bc0847 78 e_prev = error;
Miriam 0:d5fb29bc0847 79
paulineoonk 3:36e706d6b3d2 80 double ki = 0.00005; // has jet to be scaled
Miriam 0:d5fb29bc0847 81 e_int = e_int+Ts*error;
paulineoonk 3:36e706d6b3d2 82 double Integrator = ki*e_int;
Miriam 0:d5fb29bc0847 83
Miriam 0:d5fb29bc0847 84
paulineoonk 3:36e706d6b3d2 85 double motorValue = Proportional + Integrator + Derivative;
Miriam 0:d5fb29bc0847 86 return motorValue;
Miriam 0:d5fb29bc0847 87 }
paulineoonk 4:5607088ef6f5 88 */
paulineoonk 3:36e706d6b3d2 89 void SetMotor1(double motorValue)
Miriam 0:d5fb29bc0847 90 {
Miriam 0:d5fb29bc0847 91 if (motorValue >= 0)
Miriam 0:d5fb29bc0847 92 {
Miriam 0:d5fb29bc0847 93 M1D = 0;
Miriam 0:d5fb29bc0847 94 }
Miriam 0:d5fb29bc0847 95 else
Miriam 0:d5fb29bc0847 96 {
Miriam 0:d5fb29bc0847 97 M1D = 1;
Miriam 0:d5fb29bc0847 98 }
Miriam 0:d5fb29bc0847 99
Miriam 0:d5fb29bc0847 100 if (fabs(motorValue) > 1)
Miriam 0:d5fb29bc0847 101 {
Miriam 0:d5fb29bc0847 102 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Miriam 0:d5fb29bc0847 103 }
Miriam 0:d5fb29bc0847 104 else
Miriam 0:d5fb29bc0847 105 {
Miriam 0:d5fb29bc0847 106 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Miriam 0:d5fb29bc0847 107 }
Miriam 0:d5fb29bc0847 108 }
paulineoonk 3:36e706d6b3d2 109 void MeasureAndControl ()
Miriam 0:d5fb29bc0847 110 {
paulineoonk 3:36e706d6b3d2 111 // hier the control of the control system
paulineoonk 3:36e706d6b3d2 112 double refP = GetReferencePosition();
paulineoonk 4:5607088ef6f5 113 //double Huidigepositie = Encoder();
paulineoonk 4:5607088ef6f5 114 //double error = (refP - Huidigepositie);// make an error
paulineoonk 4:5607088ef6f5 115 //double motorValue = FeedBackControl(error, e_prev, e_int);
paulineoonk 4:5607088ef6f5 116 double motorValue = refP;
Miriam 0:d5fb29bc0847 117 SetMotor1(motorValue);
Miriam 0:d5fb29bc0847 118 }
Miriam 0:d5fb29bc0847 119
Miriam 0:d5fb29bc0847 120
Miriam 0:d5fb29bc0847 121 int main()
Miriam 0:d5fb29bc0847 122 {
Miriam 0:d5fb29bc0847 123 NF.add( &N1 );
Miriam 0:d5fb29bc0847 124 HPF.add( &HP1 ).add( &HP2 );
Miriam 0:d5fb29bc0847 125 LPF.add( &LP1 ).add( &LP2 );
paulineoonk 3:36e706d6b3d2 126
paulineoonk 3:36e706d6b3d2 127 Treecko.attach(MeasureAndControl, 0.1); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
paulineoonk 3:36e706d6b3d2 128 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
Miriam 0:d5fb29bc0847 129
paulineoonk 3:36e706d6b3d2 130 //emgverwerkticker.attach(&emgverwerk,dt);
paulineoonk 3:36e706d6b3d2 131 //HIDinticker.attach(&ReadFilteredSignal(emgFiltered), 0.01);
paulineoonk 3:36e706d6b3d2 132 while(true)
Miriam 0:d5fb29bc0847 133 {
paulineoonk 3:36e706d6b3d2 134 }
paulineoonk 3:36e706d6b3d2 135
paulineoonk 3:36e706d6b3d2 136
paulineoonk 3:36e706d6b3d2 137 }