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

Committer:
paulineoonk
Date:
Mon Oct 30 16:09:46 2017 +0000
Revision:
6:e0e5da2c068f
Parent:
5:daa916945271
Child:
7:05c71a859d27
PROJECT WERKT NIET --> MIRIAM HELP

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