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@0:d5fb29bc0847, 2017-10-23 (annotated)

Committer:
Miriam
Date:
Mon Oct 23 10:04:37 2017 +0000
Revision:
0:d5fb29bc0847
Child:
1:99754fe781b0
samenvoegen filteren en week6ordenenscript

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
Miriam 0:d5fb29bc0847 8
Miriam 0:d5fb29bc0847 9 //globale variabelen FILTERS
Miriam 0:d5fb29bc0847 10
Miriam 0:d5fb29bc0847 11 //Hidscope aanmaken
Miriam 0:d5fb29bc0847 12 HIDScope scope(2);
Miriam 0:d5fb29bc0847 13 double maxi = 0.12; // max signal after filtering, 0.1-0.12
Miriam 0:d5fb29bc0847 14
Miriam 0:d5fb29bc0847 15 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
Miriam 0:d5fb29bc0847 16 BiQuad N1( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
Miriam 0:d5fb29bc0847 17 BiQuadChain NF;
Miriam 0:d5fb29bc0847 18 BiQuad HP1( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
Miriam 0:d5fb29bc0847 19 BiQuad HP2( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 );
Miriam 0:d5fb29bc0847 20 BiQuadChain HPF;
Miriam 0:d5fb29bc0847 21 BiQuad LP1( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
Miriam 0:d5fb29bc0847 22 BiQuad LP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
Miriam 0:d5fb29bc0847 23 BiQuadChain LPF;
Miriam 0:d5fb29bc0847 24
Miriam 0:d5fb29bc0847 25 float f = 500; // frequency
Miriam 0:d5fb29bc0847 26 float dt = 1/f; // sample frequency
Miriam 0:d5fb29bc0847 27 Ticker emgverwerkticker;
Miriam 0:d5fb29bc0847 28 AnalogIn emg(A0); // EMG lezen
Miriam 0:d5fb29bc0847 29
Miriam 0:d5fb29bc0847 30 // globale variabelen PID controller
Miriam 0:d5fb29bc0847 31
Miriam 0:d5fb29bc0847 32 Ticker AInTicker; //We make a ticker named AIn (use for HIDScope)
Miriam 0:d5fb29bc0847 33
Miriam 0:d5fb29bc0847 34 Ticker Treecko; //We make a awesome ticker for our control system
Miriam 0:d5fb29bc0847 35 //AnalogIn potMeter2(A1); //Analoge input of potmeter 2 (will be use for te reference position) --> emgFiltered
Miriam 0:d5fb29bc0847 36 PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed
Miriam 0:d5fb29bc0847 37 DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control
Miriam 0:d5fb29bc0847 38
Miriam 0:d5fb29bc0847 39 Encoder motor1(D13,D12,true);
Miriam 0:d5fb29bc0847 40 MODSERIAL pc(USBTX,USBRX);
Miriam 0:d5fb29bc0847 41
Miriam 0:d5fb29bc0847 42 float PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
Miriam 0:d5fb29bc0847 43 const float Ts = 0.1; // tickettijd/ sample time
Miriam 0:d5fb29bc0847 44 float e_prev = 0;
Miriam 0:d5fb29bc0847 45 float e_int = 0;
Miriam 0:d5fb29bc0847 46
Miriam 0:d5fb29bc0847 47
Miriam 0:d5fb29bc0847 48 //FILTERS
Miriam 0:d5fb29bc0847 49 void emgverwerk ()
Miriam 0:d5fb29bc0847 50 {
Miriam 0:d5fb29bc0847 51 double emgNotch = NF.step(emg.read() ); // Notch filter
Miriam 0:d5fb29bc0847 52 double emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0.
Miriam 0:d5fb29bc0847 53 double emgAbsHP = abs(emgHP); // Take absolute value
Miriam 0:d5fb29bc0847 54 double emgLP = LPF.step(emgAbsHP); // Low-pass filter: creates envelope
Miriam 0:d5fb29bc0847 55 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 56 double emgFiltered = emgLP/emgMax; // Scale to maximum signal: useful for motor
Miriam 0:d5fb29bc0847 57 if (emgFiltered >1)
Miriam 0:d5fb29bc0847 58 {
Miriam 0:d5fb29bc0847 59 emgFiltered=1.00;
Miriam 0:d5fb29bc0847 60 }
Miriam 0:d5fb29bc0847 61 scope.set(0,emgFiltered);
Miriam 0:d5fb29bc0847 62 scope.set(1,emg.read());
Miriam 0:d5fb29bc0847 63 scope.send();
Miriam 0:d5fb29bc0847 64 }
Miriam 0:d5fb29bc0847 65
Miriam 0:d5fb29bc0847 66
Miriam 0:d5fb29bc0847 67 // PID CONTROLLER
Miriam 0:d5fb29bc0847 68 float GetReferencePosition()
Miriam 0:d5fb29bc0847 69 {
Miriam 0:d5fb29bc0847 70 float Potmeterwaarde = potMeter2.read();
Miriam 0:d5fb29bc0847 71 int maxwaarde = 4096; // = 64x64
Miriam 0:d5fb29bc0847 72 float refP = Potmeterwaarde*maxwaarde;
Miriam 0:d5fb29bc0847 73 return refP; // value between 0 and 4096
Miriam 0:d5fb29bc0847 74 }
Miriam 0:d5fb29bc0847 75
Miriam 0:d5fb29bc0847 76 float FeedBackControl(float error, float &e_prev, float &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
Miriam 0:d5fb29bc0847 77 {
Miriam 0:d5fb29bc0847 78 float kp = 0.001; // kind of scaled.
Miriam 0:d5fb29bc0847 79 float Proportional= kp*error;
Miriam 0:d5fb29bc0847 80
Miriam 0:d5fb29bc0847 81 float kd = 0.0004; // kind of scaled.
Miriam 0:d5fb29bc0847 82 float VelocityError = (error - e_prev)/Ts;
Miriam 0:d5fb29bc0847 83 float Derivative = kd*VelocityError;
Miriam 0:d5fb29bc0847 84 e_prev = error;
Miriam 0:d5fb29bc0847 85
Miriam 0:d5fb29bc0847 86 float ki = 0.00005; // kind of scaled.
Miriam 0:d5fb29bc0847 87 e_int = e_int+Ts*error;
Miriam 0:d5fb29bc0847 88 float Integrator = ki*e_int;
Miriam 0:d5fb29bc0847 89
Miriam 0:d5fb29bc0847 90
Miriam 0:d5fb29bc0847 91 float motorValue = Proportional + Integrator + Derivative;
Miriam 0:d5fb29bc0847 92 return motorValue;
Miriam 0:d5fb29bc0847 93 }
Miriam 0:d5fb29bc0847 94
Miriam 0:d5fb29bc0847 95 void SetMotor1(float motorValue)
Miriam 0:d5fb29bc0847 96 {
Miriam 0:d5fb29bc0847 97 if (motorValue >= 0)
Miriam 0:d5fb29bc0847 98 {
Miriam 0:d5fb29bc0847 99 M1D = 0;
Miriam 0:d5fb29bc0847 100 }
Miriam 0:d5fb29bc0847 101 else
Miriam 0:d5fb29bc0847 102 {
Miriam 0:d5fb29bc0847 103 M1D = 1;
Miriam 0:d5fb29bc0847 104 }
Miriam 0:d5fb29bc0847 105
Miriam 0:d5fb29bc0847 106 if (fabs(motorValue) > 1)
Miriam 0:d5fb29bc0847 107 {
Miriam 0:d5fb29bc0847 108 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Miriam 0:d5fb29bc0847 109 }
Miriam 0:d5fb29bc0847 110 else
Miriam 0:d5fb29bc0847 111 {
Miriam 0:d5fb29bc0847 112 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Miriam 0:d5fb29bc0847 113 }
Miriam 0:d5fb29bc0847 114 }
Miriam 0:d5fb29bc0847 115
Miriam 0:d5fb29bc0847 116 float Encoder ()
Miriam 0:d5fb29bc0847 117 {
Miriam 0:d5fb29bc0847 118 float Huidigepositie = motor1.getPosition ();
Miriam 0:d5fb29bc0847 119 return Huidigepositie; // huidige positie = current position
Miriam 0:d5fb29bc0847 120 }
Miriam 0:d5fb29bc0847 121
Miriam 0:d5fb29bc0847 122 void MeasureAndControl(void)
Miriam 0:d5fb29bc0847 123 {
Miriam 0:d5fb29bc0847 124 // hier the control of the control system
Miriam 0:d5fb29bc0847 125 float refP = GetReferencePosition();
Miriam 0:d5fb29bc0847 126 float Huidigepositie = Encoder();
Miriam 0:d5fb29bc0847 127 float error = (refP - Huidigepositie);// make an error
Miriam 0:d5fb29bc0847 128 float motorValue = FeedBackControl(error, e_prev, e_int);
Miriam 0:d5fb29bc0847 129 SetMotor1(motorValue);
Miriam 0:d5fb29bc0847 130 }
Miriam 0:d5fb29bc0847 131
Miriam 0:d5fb29bc0847 132
Miriam 0:d5fb29bc0847 133 int main()
Miriam 0:d5fb29bc0847 134 {
Miriam 0:d5fb29bc0847 135 NF.add( &N1 );
Miriam 0:d5fb29bc0847 136 HPF.add( &HP1 ).add( &HP2 );
Miriam 0:d5fb29bc0847 137 LPF.add( &LP1 ).add( &LP2 );
Miriam 0:d5fb29bc0847 138 emgverwerkticker.attach(&emgverwerk,dt);
Miriam 0:d5fb29bc0847 139
Miriam 0:d5fb29bc0847 140 M1E.period(PwmPeriod);
Miriam 0:d5fb29bc0847 141 Treecko.attach(MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
Miriam 0:d5fb29bc0847 142 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
Miriam 0:d5fb29bc0847 143
Miriam 0:d5fb29bc0847 144 while(1)
Miriam 0:d5fb29bc0847 145 {
Miriam 0:d5fb29bc0847 146 wait(0.2);
Miriam 0:d5fb29bc0847 147 pc.baud(115200);
Miriam 0:d5fb29bc0847 148 float B = motor1.getPosition();
Miriam 0:d5fb29bc0847 149 float Potmeterwaarde = potMeter2.read();
Miriam 0:d5fb29bc0847 150 //float positie = B%4096;
Miriam 0:d5fb29bc0847 151 pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V
Miriam 0:d5fb29bc0847 152 }
Miriam 0:d5fb29bc0847 153 }