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@1:99754fe781b0, 2017-10-23 (annotated)

Committer:
Miriam
Date:
Mon Oct 23 10:34:30 2017 +0000
Revision:
1:99754fe781b0
Parent:
0:d5fb29bc0847
Child:
2:bc01b1ce8fb6
probeersel intergratie 1

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 1:99754fe781b0 46 float refP = 0;
Miriam 0:d5fb29bc0847 47
Miriam 0:d5fb29bc0847 48 //FILTERS
Miriam 1:99754fe781b0 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 1:99754fe781b0 65 */
Miriam 0:d5fb29bc0847 66
Miriam 0:d5fb29bc0847 67
Miriam 0:d5fb29bc0847 68 // PID CONTROLLER
Miriam 1:99754fe781b0 69 float GetReferencePosition(double emgFilt, float refP ) // anders met emg
Miriam 0:d5fb29bc0847 70 {
Miriam 1:99754fe781b0 71 if (.45<emgFilt<.80)
Miriam 1:99754fe781b0 72 {
Miriam 1:99754fe781b0 73 refP += 1;
Miriam 1:99754fe781b0 74 }
Miriam 1:99754fe781b0 75 else if (emgFilt>=.80)
Miriam 1:99754fe781b0 76 {
Miriam 1:99754fe781b0 77 refP += 4;
Miriam 1:99754fe781b0 78 }
Miriam 1:99754fe781b0 79 else
Miriam 1:99754fe781b0 80 { }
Miriam 1:99754fe781b0 81
Miriam 1:99754fe781b0 82 int maxwaarde = 4096; // = 64x64 aantal posities die de motor kan hebben
Miriam 1:99754fe781b0 83 float refPos = refP*maxwaarde;
Miriam 1:99754fe781b0 84 return refPos; // value between 0 and 4096
Miriam 0:d5fb29bc0847 85 }
Miriam 0:d5fb29bc0847 86
Miriam 0:d5fb29bc0847 87 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 88 {
Miriam 0:d5fb29bc0847 89 float kp = 0.001; // kind of scaled.
Miriam 0:d5fb29bc0847 90 float Proportional= kp*error;
Miriam 0:d5fb29bc0847 91
Miriam 0:d5fb29bc0847 92 float kd = 0.0004; // kind of scaled.
Miriam 0:d5fb29bc0847 93 float VelocityError = (error - e_prev)/Ts;
Miriam 0:d5fb29bc0847 94 float Derivative = kd*VelocityError;
Miriam 0:d5fb29bc0847 95 e_prev = error;
Miriam 0:d5fb29bc0847 96
Miriam 0:d5fb29bc0847 97 float ki = 0.00005; // kind of scaled.
Miriam 0:d5fb29bc0847 98 e_int = e_int+Ts*error;
Miriam 0:d5fb29bc0847 99 float Integrator = ki*e_int;
Miriam 0:d5fb29bc0847 100
Miriam 0:d5fb29bc0847 101
Miriam 0:d5fb29bc0847 102 float motorValue = Proportional + Integrator + Derivative;
Miriam 0:d5fb29bc0847 103 return motorValue;
Miriam 0:d5fb29bc0847 104 }
Miriam 0:d5fb29bc0847 105
Miriam 0:d5fb29bc0847 106 void SetMotor1(float motorValue)
Miriam 0:d5fb29bc0847 107 {
Miriam 0:d5fb29bc0847 108 if (motorValue >= 0)
Miriam 0:d5fb29bc0847 109 {
Miriam 0:d5fb29bc0847 110 M1D = 0;
Miriam 0:d5fb29bc0847 111 }
Miriam 0:d5fb29bc0847 112 else
Miriam 0:d5fb29bc0847 113 {
Miriam 0:d5fb29bc0847 114 M1D = 1;
Miriam 0:d5fb29bc0847 115 }
Miriam 0:d5fb29bc0847 116
Miriam 0:d5fb29bc0847 117 if (fabs(motorValue) > 1)
Miriam 0:d5fb29bc0847 118 {
Miriam 0:d5fb29bc0847 119 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Miriam 0:d5fb29bc0847 120 }
Miriam 0:d5fb29bc0847 121 else
Miriam 0:d5fb29bc0847 122 {
Miriam 0:d5fb29bc0847 123 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Miriam 0:d5fb29bc0847 124 }
Miriam 0:d5fb29bc0847 125 }
Miriam 0:d5fb29bc0847 126
Miriam 0:d5fb29bc0847 127 float Encoder ()
Miriam 0:d5fb29bc0847 128 {
Miriam 0:d5fb29bc0847 129 float Huidigepositie = motor1.getPosition ();
Miriam 0:d5fb29bc0847 130 return Huidigepositie; // huidige positie = current position
Miriam 0:d5fb29bc0847 131 }
Miriam 0:d5fb29bc0847 132
Miriam 0:d5fb29bc0847 133 void MeasureAndControl(void)
Miriam 0:d5fb29bc0847 134 {
Miriam 1:99754fe781b0 135 //emgverwerken
Miriam 1:99754fe781b0 136 double emgNotch = NF.step(emg.read() ); // Notch filter
Miriam 1:99754fe781b0 137 double emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0.
Miriam 1:99754fe781b0 138 double emgAbsHP = abs(emgHP); // Take absolute value
Miriam 1:99754fe781b0 139 double emgLP = LPF.step(emgAbsHP); // Low-pass filter: creates envelope
Miriam 1:99754fe781b0 140 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 1:99754fe781b0 141 double emgFiltered = emgLP/emgMax; // Scale to maximum signal: useful for motor
Miriam 1:99754fe781b0 142 if (emgFiltered >1)
Miriam 1:99754fe781b0 143 {
Miriam 1:99754fe781b0 144 emgFiltered=1.00;
Miriam 1:99754fe781b0 145 }
Miriam 1:99754fe781b0 146 scope.set(0,emgFiltered);
Miriam 1:99754fe781b0 147 scope.set(1,emg.read());
Miriam 1:99754fe781b0 148 scope.send();
Miriam 1:99754fe781b0 149
Miriam 0:d5fb29bc0847 150 // hier the control of the control system
Miriam 1:99754fe781b0 151 float refPos = GetReferencePosition(emgFiltered, refP);
Miriam 0:d5fb29bc0847 152 float Huidigepositie = Encoder();
Miriam 0:d5fb29bc0847 153 float error = (refP - Huidigepositie);// make an error
Miriam 0:d5fb29bc0847 154 float motorValue = FeedBackControl(error, e_prev, e_int);
Miriam 0:d5fb29bc0847 155 SetMotor1(motorValue);
Miriam 0:d5fb29bc0847 156 }
Miriam 0:d5fb29bc0847 157
Miriam 0:d5fb29bc0847 158
Miriam 0:d5fb29bc0847 159 int main()
Miriam 0:d5fb29bc0847 160 {
Miriam 0:d5fb29bc0847 161 NF.add( &N1 );
Miriam 0:d5fb29bc0847 162 HPF.add( &HP1 ).add( &HP2 );
Miriam 0:d5fb29bc0847 163 LPF.add( &LP1 ).add( &LP2 );
Miriam 1:99754fe781b0 164 //emgverwerkticker.attach(&emgverwerk, dt);
Miriam 0:d5fb29bc0847 165
Miriam 0:d5fb29bc0847 166 M1E.period(PwmPeriod);
Miriam 1:99754fe781b0 167 Treecko.attach(MeasureAndControl, dt); //Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
Miriam 0:d5fb29bc0847 168 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
Miriam 0:d5fb29bc0847 169
Miriam 0:d5fb29bc0847 170 while(1)
Miriam 0:d5fb29bc0847 171 {
Miriam 0:d5fb29bc0847 172 wait(0.2);
Miriam 0:d5fb29bc0847 173 pc.baud(115200);
Miriam 0:d5fb29bc0847 174 float B = motor1.getPosition();
Miriam 1:99754fe781b0 175 //float Potmeterwaarde = potMeter2.read();
Miriam 0:d5fb29bc0847 176 //float positie = B%4096;
Miriam 1:99754fe781b0 177 //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 178 }
Miriam 0:d5fb29bc0847 179 }