Miriam van der Hoek / Mbed 2 deprecated vanEMGnaarMOTOR

intergreren van de twee scriptjes (emgfilters en PID controller). kijken of de motor aan te sturen is met emg-signalen

Dependencies:   Encoder HIDScope MODSERIAL biquadFilter mbed

main.cpp@2:bc01b1ce8fb6, 2017-10-23 (annotated)

Committer:
Miriam
Date:
Mon Oct 23 11:18:57 2017 +0000
Revision:
2:bc01b1ce8fb6
Parent:
1:99754fe781b0 
probleem, gaat terug naar nul, waarschijnlijk een probleem in 1 van de vele refP.

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