to send emg signal to motor with test programm
Dependencies: HIDScope biquadFilter mbed
Fork of TestProgramm by
Diff: main.cpp
- Revision:
- 29:a48b63e60a40
- Parent:
- 28:4b22455930ff
- Child:
- 30:4c6321941515
diff -r 4b22455930ff -r a48b63e60a40 main.cpp --- a/main.cpp Wed Oct 25 11:24:25 2017 +0000 +++ b/main.cpp Wed Oct 25 12:21:50 2017 +0000 @@ -8,12 +8,15 @@ //Define objects AnalogIn emg( A0 ); //EMG AnalogIn emg1( A1 ); //EMG -HIDScope scope( 5 ); // aantal scopes dat gemaakt kan worden +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); int P= 200; // aantal test punten voor de moving average @@ -22,6 +25,8 @@ 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; // Filters BiQuadChain bqc; @@ -54,7 +59,7 @@ sum = sum + A[n]; } - double movmean = sum/P; //dit is de moving average waarde + 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) { @@ -86,13 +91,47 @@ x=0; } } - + double EMGInput = (movmean - Vwaarde[0]*3)/(Vwaarde[1] - Vwaarde[0]*3); + scope.set(5,EMGInput); scope.set(2, movmean); // stuurt de moving average naar de plot 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 = ((movmean - Vwaarde[0]*3)/(Vwaarde[1] - Vwaarde[0]*3) * 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; + } +} int main() { @@ -100,6 +139,11 @@ 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) {} + while(1) { + MotorOn.rise(&MotorLocker); + motor2DirectionPin = 1; + SetMotor2(FeedForwardControl(GetReferenceVelocity())); + } } \ No newline at end of file