Ralph Gerlings
try this for a change
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
EMG_5
by 
Ralph Gerlings
Diff: main.cpp
- Revision:
- 31:d346f9244b4a
- Parent:
- 30:2c67abcdb892
- Child:
- 32:a779b1131977
--- a/main.cpp Mon Oct 30 14:34:55 2017 +0000
+++ b/main.cpp Wed Nov 01 11:34:46 2017 +0000
@@ -2,19 +2,27 @@
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
-#include "QEI.h"
+//#include "QEI.h"
//Define objects
- AnalogIn emg1_in( A0 ); /* read out the signal */
+ AnalogIn emg1_in( A0 ); // read out the signal
AnalogIn emg2_in( A1 );
AnalogIn emg3_in( A2 );
AnalogIn emg4_in( A3 );
- DigitalIn max_reader12( SW2 ); /* define button press */
+ DigitalIn max_reader12( SW2 ); // define button press
DigitalIn max_reader34( SW3 );
+
+ DigitalOut motor1direction( D4 );
+ PwmOut motor1pwm( D5);
+ PwmOut motor2pwm( D6 );
+ DigitalOut motor2direction( D7 );
+ //QEI Encoder1(D10, D11, NC, 32); // Encoder reminder
+ //QEI Encoder2(D12, D13, NC, 32);
Ticker main_timer;
Ticker max_read1;
Ticker max_read3;
+ Ticker Motorcontrol;
HIDScope scope( 4 );
DigitalOut red(LED_RED);
DigitalOut blue(LED_BLUE);
@@ -24,14 +32,11 @@
// EMG variables
//Right Biceps
-
-
double emg1;
double emg1highfilter;
double emg1notchfilter;
double emg1abs;
double emg1lowfilter;
- double emg1peak;
double max1;
double maxpart1;
// Left Biceps
@@ -40,7 +45,6 @@
double emg2notchfilter;
double emg2abs;
double emg2lowfilter;
- double emg2peak;
double max2;
double maxpart2;
// Left Lower Arm
@@ -49,7 +53,6 @@
double emg3notchfilter;
double emg3abs;
double emg3lowfilter;
- double emg3peak;
double max3;
double maxpart3;
// Right Lower Arm
@@ -58,70 +61,73 @@
double emg4notchfilter;
double emg4abs;
double emg4lowfilter;
- double emg4peak;
double max4;
double maxpart4;
+
+// Motor variables
+ float referenceVelocity;
+ float potMeterIn;
+ float MV1 = 0;
+ float MV2 = 0;
// BiQuad Filter Settings
// Right Biceps
- BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */
- BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); /* Filter at 50 Hz */
- BiQuad filterpeak1(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); /* 4dB Gain peak at 11 Hz */
- BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); /* Filter at 15 Hz */
+ BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); // Filter at 10 Hz
+ BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); // Filter at 50 Hz
+ BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); // Filter at 15 Hz
// Left Biceps
BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
- BiQuad filterpeak2(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
// Left Lower Arm OR Triceps
BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
- BiQuad filterpeak3(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
// Right Lower Arm OR Triceps
BiQuad filterhigh4(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
BiQuad filternotch4(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
- BiQuad filterpeak4(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
//
// Finding max values for correct motor switch if the button is pressed
+// calibration of both biceps
+
void get_max1(){
if (max_reader12==0){
green = !green;
red = 1;
blue = 1;
- for(int n=0;n<2000;n++){ /* measure 2000 samples and filter it */
+ for(int n=0;n<2000;n++){ // measure 2000 samples and filter it
- emg1 = emg1_in.read(); /* read out emg */
- emg1highfilter = filterhigh1.step(emg1); /* high pass filtered */
- emg1notchfilter = filternotch1.step(emg1highfilter); /* notch filtered */
- emg1abs = fabs(emg1notchfilter); /* take the absolute value */
- emg1lowfilter = filterlow1.step(emg1abs); /* low pass filtered */
- emg1peak = filterpeak1.step(emg1lowfilter); /* 4dB gain peak */
+ emg1 = emg1_in.read(); // read out emg
+ emg1highfilter = filterhigh1.step(emg1); // high pass filtered
+ emg1notchfilter = filternotch1.step(emg1highfilter); // notch filtered
+ emg1abs = fabs(emg1notchfilter); // take the absolute value
+ emg1lowfilter = filterlow1.step(emg1abs); // low pass filtered
- emg2 = emg2_in.read(); /* read out emg */
- emg2highfilter = filterhigh2.step(emg2); /* high pass filtered */
- emg2notchfilter = filternotch2.step(emg2highfilter); /* notch filtered */
- emg2abs = fabs(emg2notchfilter); /* take the absolute value */
- emg2lowfilter = filterlow2.step(emg2abs); /* low pass filtered */
- emg2peak = filterpeak2.step(emg2lowfilter); /* 4dB gain peak */
+ emg2 = emg2_in.read(); // read out emg
+ emg2highfilter = filterhigh2.step(emg2); // high pass filtered
+ emg2notchfilter = filternotch2.step(emg2highfilter); // notch filtered
+ emg2abs = fabs(emg2notchfilter); // take the absolute value
+ emg2lowfilter = filterlow2.step(emg2abs); // low pass filtered
- if (max1<emg1peak){
- max1 = emg1peak; /* set the max value at the highest measured value */
+ if (max1<emg1lowfilter){
+ max1 = emg1lowfilter; // set the max value at the highest measured value
}
- if (max2<emg2peak){
- max2 = emg2peak; /* set the max value at the highest measured value */
+ if (max2<emg2lowfilter){
+ max2 = emg2lowfilter; // set the max value at the highest measured value
}
- wait(0.001f); /* measure at 1000Hz */
+ wait(0.001f); // measure at 1000Hz
}
wait(0.2f);
green = 1;
}
- maxpart1 = 0.12*max1; /* set cut off voltage at 15% of max for right biceps */
- maxpart2 = 0.12*max2; /* set cut off votage at 15% of max for left biceps */
+ maxpart1 = 0.11*max1; // set cut off voltage at 13% of max for right biceps
+ maxpart2 = 0.13*max2; // set cut off voltage at 13% of max for left biceps
}
+// calibration of both lower arms
+
void get_max3(){
if (max_reader34==0){
green = 1;
@@ -134,28 +140,26 @@
emg3notchfilter = filternotch3.step(emg3highfilter);
emg3abs = fabs(emg3notchfilter);
emg3lowfilter = filterlow3.step(emg3abs);
- emg3peak = filterpeak3.step(emg3lowfilter);
emg4 = emg4_in.read();
emg4highfilter = filterhigh4.step(emg4);
emg4notchfilter = filternotch4.step(emg4highfilter);
emg4abs = fabs(emg4notchfilter);
emg4lowfilter = filterlow4.step(emg4abs);
- emg4peak = filterpeak4.step(emg4lowfilter);
- if (max3<emg3peak){
- max3 = emg3peak; /* set the max value at the highest measured value */
+ if (max3<emg3lowfilter){
+ max3 = emg3lowfilter; // set the max value at the highest measured value
}
- if (max4<emg4peak){
- max4 = emg4peak; /* set the max value at the highest measured value */
+ if (max4<emg4lowfilter){
+ max4 = emg4lowfilter; // set the max value at the highest measured value
}
wait(0.001f);
}
wait(0.2f);
red = 1;
}
- maxpart3 = 0.18*max3; /* set cut off voltage at 25% of max for left lower arm */
- maxpart4 = 0.18*max4; /* set cut off voltage at 25% of max for right lower arm */
+ maxpart3 = 0.15*max3; // set cut off voltage at 15% of max for left lower arm
+ maxpart4 = 0.15*max4; // set cut off voltage at 15% of max for right lower arm
}
// Filtering & Scope
@@ -165,101 +169,147 @@
emg1highfilter = filterhigh1.step(emg1);
emg1notchfilter = filternotch1.step(emg1highfilter);
emg1abs = fabs(emg1notchfilter);
- emg1lowfilter = filterlow1.step(emg1abs);
- emg1peak = filterpeak1.step(emg1lowfilter); /* Final Right Biceps values to be sent */
+ emg1lowfilter = filterlow1.step(emg1abs); // Final Right Biceps values to be sent
// Left Biceps
emg2 = emg2_in.read();
emg2highfilter = filterhigh2.step(emg2);
emg2notchfilter = filternotch2.step(emg2highfilter);
emg2abs = fabs(emg2notchfilter);
- emg2lowfilter = filterlow2.step(emg2abs);
- emg2peak = filterpeak2.step(emg2lowfilter); /* Final Left Biceps values to be sent */
+ emg2lowfilter = filterlow2.step(emg2abs); // Final Left Biceps values to be sent
// Left Lower Arm OR Triceps
emg3 = emg3_in.read();
emg3highfilter = filterhigh3.step(emg3);
emg3notchfilter = filternotch3.step(emg3highfilter);
emg3abs = fabs(emg3notchfilter);
- emg3lowfilter = filterlow3.step(emg3abs);
- emg3peak = filterpeak3.step(emg3lowfilter); /* Final Lower Arm values to be sent */
+ emg3lowfilter = filterlow3.step(emg3abs); // Final Lower Arm values to be sent
// Right Lower Arm OR Triceps
emg4 = emg4_in.read();
emg4highfilter = filterhigh4.step(emg4);
emg4notchfilter = filternotch4.step(emg4highfilter);
emg4abs = fabs(emg4notchfilter);
- emg4lowfilter = filterlow4.step(emg4abs);
- emg4peak = filterpeak4.step(emg4lowfilter); /* Final Lower Arm values to be sent */
+ emg4lowfilter = filterlow4.step(emg4abs); // Final Lower Arm values to be sent
- /* Compare measurement to the calibrated value to decide actions */
-
- /* This part checks for right biceps contractions only*/
-if (maxpart1<emg1peak && maxpart2>emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
+ // Compare measurement to the calibrated value to decide actions
+ // more options could be added if the callibration is well defined enough
+ // This part checks for right biceps contractions only
+ if (maxpart1<emg1lowfilter && maxpart2>emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
red = 1;
blue = 1;
green = 0;
-}
- /* This part checks for left biceps contractions only*/
-else if (maxpart1>emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
+ MV1 = 0.5;
+ MV2 = 0;
+ }
+ // This part checks for left biceps contractions only
+ else if (maxpart1>emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
red = 0;
blue = 1;
green = 1;
- }
- /* This part checks for left lower arm contractions only*/
-else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4>emg4peak){
+ MV1 = -0.5;
+ MV2 = 0;
+ }
+ // This part checks for left lower arm contractions only
+ else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4>emg4lowfilter){
red = 1;
blue = 0;
green = 1;
- }
- /* This part checks for right lower arm contractions only */
-else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3>emg3peak && maxpart4<emg4peak){
+ MV1 = 0;
+ MV2 = 0.5;
+ }
+ // This part checks for right lower arm contractions only
+ else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3>emg3lowfilter && maxpart4<emg4lowfilter){
red = 0;
blue = 1;
green = 0;
- }
- /* This part checks for both lower arm contractions only */
-else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4<emg4peak){
+ MV1 = 0;
+ MV2 = -0.5;
+ }
+ // This part checks for both lower arm contractions only
+ else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4<emg4lowfilter){
+ red = 0;
+ blue = 0;
+ green = 0;
+ MV1 = -0.5;
+ MV2 = -0.5;
+ }
+ // This part checks for both biceps contractions only
+ else if (maxpart1<emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
+ red = 0;
+ blue = 0;
+ green = 0;
+ MV1 = 0.5;
+ MV2 = 0.5;
+ }
+ // This part checks for right lower arm & left biceps contractions only
+ else if (maxpart1>emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4<emg4lowfilter){
red = 0;
blue = 0;
green = 0;
- }
- /* This part checks for both biceps contractions only*/
-else if (maxpart1<emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
- red = 0;
- blue = 0;
- green = 0;
- }
- /* This part checks for right lower arm & left biceps contractions only*/
-else if (maxpart1>emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4<emg4peak){
- red = 0;
- blue = 0;
- green = 0;
- }
- /* This part checks for left lower arm & right biceps contractions only*/
-else if (maxpart1<emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4>emg4peak){
+ MV1 = 0.5;
+ MV2 = -0.5;
+ }
+ // This part checks for left lower arm & right biceps contractions only
+ else if (maxpart1<emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4>emg4lowfilter){
red = 0;
blue = 0;
green = 0;
- }
-else {
- red = 1; /* Shut down all led colors if no movement is registered */
+ MV1 = -0.5;
+ MV2 = 0.5;
+ }
+ else {
+ red = 1; // Shut down all led colors if no movement is registered
blue = 1;
green = 1;
+ MV1 = 0;
+ MV2 = 0;
//pc.printf( "No contraction registered\n");
}
- /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
- scope.set(0, emg1peak ); /* plot Right biceps voltage */
- scope.set(1, emg2peak ); /* Plot Left biceps voltage */
- scope.set(2, emg3peak ); /* Plot Lower Left Arm voltage */
- scope.set(3, emg4peak ); /* Plot Lower Right Arm Voltage */
-
- scope.send(); /* send everything to the HID scope */
+ // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
+ scope.set(0, emg1lowfilter ); // plot Right biceps voltage
+ scope.set(1, emg2lowfilter ); // Plot Left biceps voltage
+ scope.set(2, emg3lowfilter ); // Plot Lower Left Arm voltage
+ scope.set(3, emg4lowfilter ); // Plot Lower Right Arm Voltage
+ scope.send(); // send everything to the HID scope
+
}
+// check MV1 to see if motor1 needs to be activated
+void SetMotor1(float MV1) {
+ motor1pwm = fabs(MV1); // motor speed
+ if (MV1 >=0) {
+ motor1direction = 1; // clockwise rotation
+ }
+ else {
+ motor1direction = 0; // counterclockwise rotation
+ }
+}
+// check MV2 if motor1 needs to be activated
+void SetMotor2(float MV2) {
+ motor2pwm = fabs(MV2); // motor speed
+ if (MV2 >=0) {
+ motor2direction = 1; // clockwise rotation
+ }
+ else {
+ motor2direction = 0; // counterclockwise rotation
+ }
+}
+
+void MeasureAndControl(void)
+{
+ // This function measures the potmeter position, extracts a
+ // reference velocity from it, and controls the motor with
+ // a simple FeedForward controller. Call this from a Ticker.
+ SetMotor1(MV1);
+ SetMotor2(MV2);
+}
+
+
int main(){
- main_timer.attach(&filter, 0.001); /* set frequency for the filters at 1000Hz */
- max_read1.attach(&get_max1, 2); /* set the frequency of the calibration loop at 0.5Hz */
+ main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
+ max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
max_read3.attach(&get_max3, 2);
+ Motorcontrol.attach(MeasureAndControl,0.5);
while(1) {}
}
\ No newline at end of file