to send emg signal to motor with test programm
Dependencies: HIDScope biquadFilter mbed
Fork of EMGvoorjan by
main.cpp
- Committer:
- Roytsg
- Date:
- 2017-10-26
- Revision:
- 33:84d986230e15
- Parent:
- 32:3832f732f17a
- Child:
- 34:008bd226d37e
File content as of revision 33:84d986230e15:
#include "mbed.h" #include "HIDScope.h" #include "BiQuad.h" #include "math.h" //Define objects AnalogIn emg( A0 ); //EMG AnalogIn emg1( A1 ); //EMG HIDScope scope( 6 ); // aantal scopes dat gemaakt kan worden DigitalOut ledB(LED_BLUE); DigitalOut ledG(LED_GREEN); DigitalIn TestButton(PTA4); // button naast het ledje DigitalIn onoff(PTC6); // button aan de andere kant Ticker emgSampleTicker; // Ticker voor de sample frequency DigitalOut motor2MagnitudePin(D5); // magnitude motor 2 DigitalOut motor2DirectionPin(D4); // direction of the motor 2 InterruptIn MotorOn(D10); InterruptIn SpeedUp(D12); int P= 200; // aantal test punten voor de moving average double A[200]; // de vector waar punten in worden opgeslagen voor de moving average moet even groot zijn als P int k = 0; // counter voor de configuratie double Vvector[200]; // vector voor de Vwaarde configuratie double Vwaarde[2]; // vector voor waardes van V int x = 0; // x waarde voor de Vwaarde double movmean; int MotorLock = 0; double EMGInput; float ErrorZero = 3; double MotorSpeed = 0.5; // Filters BiQuadChain bqc; BiQuad bq1( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp? BiQuad bq2( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306, -1.2243497755555959, 0.5133795508233266); //hp? BiQuad bq3( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633, -1.2243497755555959, 0.5133795508233266); // notch? // sample function voor plotten van de emg signalen en moving average void emgSample() { double emgFiltered = bqc.step( emg.read() ); // gefilterde waarde van het emg signaal double emgabs = abs(emgFiltered); // absolute waarde van het gefilterde signaal scope.set(0, emgFiltered ); // stuurt de waarden naar de grafiek scope.set(1, emgabs ); // stuurt de waarden naar de grafiek Vwaarde[0] = 0.01; // deze for-loop maakt de vector voor de moving average for(int i = P-1; i >= 0; i--){ if (i == 0) { A[i] = emgabs; } else { A[i] = A[i-1]; } } double sum = 0; // deze for-loop sommeert de array for (int n = 0; n < P-1; n++) { sum = sum + A[n]; } movmean = sum/P; //dit is de moving average waarde // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen if (TestButton==0 & k<200) { Vvector[k] = movmean; ledB = !ledB; k++; } else if (k==200) { // hier moet de test klaar zijn double sumZ = 0; for (int n = 0; n < 199; n++) { sumZ = sumZ + Vvector[n]; } // neemt de som van de zerovector array Vwaarde[x] = sumZ/200; // dit is het gemiddelde voor een betrouwbare value scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje if (x == 1) { scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje } k++; ledB = 1; ledG = !ledG; } else if (k == 201 && onoff ==0) {// dit is om het ledje uit te doen en om het mogelijk te maken de test opnieuw te doen ledG = !ledG; k = 0; if (x==0) { x++; } else if (x==1) { x=0; } } scope.set(2, movmean); // stuurt de moving average naar de plot if (movmean > Vwaarde[1]) { movmean = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn } double EMGInputRaw = (movmean - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); if (EMGInputRaw < 0) { EMGInput = 0; } else { EMGInput = EMGInputRaw; } scope.set(5,EMGInput); scope.send(); } float GetReferenceVelocity() { // Returns reference velocity in rad/s. // Positive value means clockwise rotation. const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s referenceVelocity = (EMGInput * maxVelocity) * MotorLock; return referenceVelocity; } void SetMotor2(float motorValue) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (fabs(motorValue)>1) motor2MagnitudePin = 1; else motor2MagnitudePin = fabs(motorValue); } float FeedForwardControl(float referenceVelocity) { // very simple linear feed-forward control const float MotorGain=8.4; // unit: (rad/s) / PWM float motorValue = referenceVelocity / MotorGain; return motorValue; } void MotorLocker() { if (MotorLock == 0) { MotorLock = 1; } else if (MotorLock == 1) { MotorLock = 0; } } void TestMotorSpeed() { if (MotorSpeed == 0.5) { MotorSpeed = 1.0; } else if (MotorSpeed == 1.0) { MotorSpeed == 0.5; } } int main() { ledB = 1; ledG = 1; bqc.add( &bq1 ).add( &bq2 ).add( &bq3 ); // hier wordt het filter gemaakt emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz while(1) { MotorOn.rise(&MotorLocker); motor2DirectionPin = 1; //SetMotor2(FeedForwardControl(GetReferenceVelocity())); SpeedUp.rise(&TestMotorSpeed); motor2MagnitudePin = MotorSpeed; } }