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:
- 2:bc01b1ce8fb6
- Parent:
- 1:99754fe781b0
--- a/main.cpp Mon Oct 23 10:34:30 2017 +0000 +++ b/main.cpp Mon Oct 23 11:18:57 2017 +0000 @@ -43,7 +43,7 @@ const float Ts = 0.1; // tickettijd/ sample time float e_prev = 0; float e_int = 0; -float refP = 0; +float refP_prev = 0; //FILTERS /*void emgverwerk () @@ -66,22 +66,23 @@ // PID CONTROLLER -float GetReferencePosition(double emgFilt, float refP ) // anders met emg +float GetReferencePosition(double emgFilt, double &refP_prev ) // anders met emg { + double refP; if (.45<emgFilt<.80) { - refP += 1; + refP= refP_prev + 0.001; } else if (emgFilt>=.80) { - refP += 4; + refP= refP_prev + 0.04; } else { } int maxwaarde = 4096; // = 64x64 aantal posities die de motor kan hebben - float refPos = refP*maxwaarde; - return refPos; // value between 0 and 4096 + refP = refP*maxwaarde; + return refP; // 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) @@ -148,7 +149,7 @@ scope.send(); // hier the control of the control system - float refPos = GetReferencePosition(emgFiltered, refP); + float refP = GetReferencePosition(emgFiltered, refP_prev); float Huidigepositie = Encoder(); float error = (refP - Huidigepositie);// make an error float motorValue = FeedBackControl(error, e_prev, e_int);