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