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
Diff: main.cpp
- Revision:
- 1:99754fe781b0
- Parent:
- 0:d5fb29bc0847
- Child:
- 2:bc01b1ce8fb6
--- a/main.cpp Mon Oct 23 10:04:37 2017 +0000 +++ b/main.cpp Mon Oct 23 10:34:30 2017 +0000 @@ -43,10 +43,10 @@ const float Ts = 0.1; // tickettijd/ sample time float e_prev = 0; float e_int = 0; - +float refP = 0; //FILTERS -void emgverwerk () +/*void emgverwerk () { double emgNotch = NF.step(emg.read() ); // Notch filter double emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0. @@ -62,15 +62,26 @@ scope.set(1,emg.read()); scope.send(); } +*/ // PID CONTROLLER -float GetReferencePosition() +float GetReferencePosition(double emgFilt, float refP ) // anders met emg { - float Potmeterwaarde = potMeter2.read(); - int maxwaarde = 4096; // = 64x64 - float refP = Potmeterwaarde*maxwaarde; - return refP; // value between 0 and 4096 + if (.45<emgFilt<.80) + { + refP += 1; + } + else if (emgFilt>=.80) + { + refP += 4; + } + else + { } + + int maxwaarde = 4096; // = 64x64 aantal posities die de motor kan hebben + float refPos = refP*maxwaarde; + return refPos; // value between 0 and 4096 } 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) @@ -121,8 +132,23 @@ void MeasureAndControl(void) { + //emgverwerken + double emgNotch = NF.step(emg.read() ); // Notch filter + double emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0. + double emgAbsHP = abs(emgHP); // Take absolute value + double emgLP = LPF.step(emgAbsHP); // Low-pass filter: creates envelope + 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. + double emgFiltered = emgLP/emgMax; // Scale to maximum signal: useful for motor + if (emgFiltered >1) + { + emgFiltered=1.00; + } + scope.set(0,emgFiltered); + scope.set(1,emg.read()); + scope.send(); + // hier the control of the control system - float refP = GetReferencePosition(); + float refPos = GetReferencePosition(emgFiltered, refP); float Huidigepositie = Encoder(); float error = (refP - Huidigepositie);// make an error float motorValue = FeedBackControl(error, e_prev, e_int); @@ -135,10 +161,10 @@ NF.add( &N1 ); HPF.add( &HP1 ).add( &HP2 ); LPF.add( &LP1 ).add( &LP2 ); - emgverwerkticker.attach(&emgverwerk,dt); + //emgverwerkticker.attach(&emgverwerk, dt); M1E.period(PwmPeriod); - Treecko.attach(MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende + Treecko.attach(MeasureAndControl, dt); //Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. while(1) @@ -146,8 +172,8 @@ wait(0.2); pc.baud(115200); float B = motor1.getPosition(); - float Potmeterwaarde = potMeter2.read(); + //float Potmeterwaarde = potMeter2.read(); //float positie = B%4096; - 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 + //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 } }