Diff: main.cpp

Revision:

22:5d956c93b3ae

Parent:

21:1da43fdbd254

Child:

23:97a976a8f0fc

--- a/main.cpp	Mon Oct 22 13:44:40 2018 +0000
+++ b/main.cpp	Mon Oct 29 14:58:25 2018 +0000
@@ -4,155 +4,84 @@
 #include "HIDScope.h"
 #include <math.h>
 
-AnalogIn emg0_in            (A0);
-AnalogIn emg1_in            (A1);
-AnalogIn emg2_in            (A2);
+//ATTENTION:    set mBed to version 151
+//              set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
+//              set MODSERIAL to version 44
+//              set HIDScope to version 7
+//              set biquadFilter to version 7
 
-InterruptIn button1         (D10);                  //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
+AnalogIn emg0_in            (A0);                   //First raw EMG signal input
+AnalogIn emg1_in            (A1);                   //Second raw EMG signal input
+AnalogIn emg2_in            (A2);                   //Third raw EMG signal input
+
+InterruptIn button1         (D10);                  //Is this one available? We need to make a map of which pins are used for what.
 InterruptIn button2         (D11);
-InterruptIn encoderA        (D9);
-InterruptIn encoderB        (D8);
 
-DigitalOut directionpin1    (D4);
+DigitalOut directionpin1    (D4);                   //Motor direction pin
 DigitalOut directionpin2    (D7);
+
 DigitalOut ledr             (LED_RED);
 DigitalOut ledb             (LED_BLUE);
 DigitalOut ledg             (LED_GREEN);
 
-PwmOut pwmpin1              (D5);
+PwmOut pwmpin1              (D5);                   //Pulse width modulation --> speed motor
 PwmOut pwmpin2              (D6);
 
+//MODSERIAL pc(USBTX, USBRX);                       //Serial communication to see if the code works step by step, turn on if hidscope is off
 
-MODSERIAL pc(USBTX, USBRX);
+HIDScope    scope( 6 );                             //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
 
-//HIDscope
-Ticker      sample_timer;
-HIDScope    scope( 3 );
+//Tickers
+Ticker      HIDScope_tick;                          //Ticker for HIDScope
+Ticker      filter_tick;                            //Ticker for EMG filter
+Ticker      MovAg_tick;                             //Ticker to calculate Moving Average
 
 //Global variables
-int encoder     = 0;                   //Starting point encoder
-const float T   = 0.001f;              //Ticker period
+const float T   = 0.002f;                           //Ticker period
 
 //EMG filter
-double emgfilter0, emgfilter1, emgfilter2;                                                       //Filtered EMG data 0, 1 and 2
-const int windowsize = 150;                                                                         //Size of the array over which the moving average (MovAg) is calculated
+double emg0_filt, emg1_filt, emg2_filt;                                                          //Variables for filtered EMG data channel 0, 1 and 2
+double emg0_raw, emg1_raw, emg2_raw;
+double emg0_filt_x, emg1_filt_x, emg2_filt_x;
+const int windowsize = 150;                                                                      //Size of the array over which the moving average (MovAg) is calculated. (random number)
 double sum, sum1, sum2, sum3;                                                                    //variables used to sum elements in array
-double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize];  //Empty arrays to calculate MoveAg
-double movAg0,movAg1,movAg2;                                                                     //outcome of MovAg
+double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize];                //Empty arrays to calculate MoveAg
+double movAg0, movAg1, movAg2;                                                                   //outcome of MovAg (moet dit een array zijn??)
 
-//calibration
-int x = -1;
-int emg_cal = 0;
-const int sizeCal = 2000;
-double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];
-double Mean0,Mean1,Mean2;
-double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1;
+//Calibration variables
+int x = -1;                                                                                      //Start switch, colour LED is blue.
+int emg_cal = 0;                                                                                 //if emg_cal is set to 1, motors can begin to work in this code (!!)
+const int sizeCal = 1500;                                                                        //size of the dataset used for calibration, eerst 2000
+double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];                               //arrays to put the dataset of the calibration in
+double Mean0,Mean1,Mean2;                                                                        //average of maximum tightening
+double Threshold0, Threshold1, Threshold2; 
 
-
-//Biquad
-BiQuadChain emg0band;
+//Biquad                                                                                         //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
+BiQuadChain emg0filter;
 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
+BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter biquad coefficients
 
-BiQuadChain emg1band;
+BiQuadChain emg1filter;
 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
+BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter
 
-BiQuadChain emg2band;
+BiQuadChain emg2filter;
 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
-
-BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter
-BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter
 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter
 
-
-//Tickers
-Ticker filter_tick;
-Ticker MovAg_tick;
-
 //Functions
-void sample()
-{
-    /* 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, emg0_in.read() );
-    scope.set(1, emg1_in.read() );
-    scope.set(2, emg2_in.read() );
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
-    *  Ensure that enough channels are available (HIDScope scope( 2 ))
-    *  Finally, send all channels to the PC at once */
-    scope.send();
-    /* To indicate that the function is working, the LED is toggled */
-}
-
-void EMGFilter0()
-{
-    double emg0          = emg0_in.read();
-    double bandpass0     = emg0band.step(emg0);
-    double absolute0     = fabs(bandpass0);
-    double emgfilter0    = notch1.step(absolute0);
-}
-
-void EMGFilter1()
-{
-    double emg1          = emg1_in.read();
-    double bandpass1     = emg1band.step(emg1);
-    double absolute1     = fabs(bandpass1);
-    double emgfilter1    = notch2.step(absolute1);
-}
-
-void EMGFilter2()
-{
-    double emg2          = emg2_in.read();
-    double bandpass2     = emg2band.step(emg2);
-    double absolute2     = fabs(bandpass2);
-    double emgfilter2    = notch3.step(absolute2);
-}
-
-void MovAg()                                        //Calculate moving average (MovAg)
-{
-    for (int i = windowsize-1; i>=0; i--)            //Make array of the last datapoints of the filtered signal
-    {
-        StoreArray0[i] = StoreArray0[i-1];
-        StoreArray1[i] = StoreArray1[i-1];
-        StoreArray2[i] = StoreArray2[i-1];
-    }
-    
-    StoreArray0[0] = emgfilter0;                    //Stores the latest datapoint in the first element of the array
-    StoreArray1[0] = emgfilter1;
-    StoreArray2[0] = emgfilter2;
-    
-    sum1 = 0.0;
-    sum2 = 0.0;
-    sum3 = 0.0;
-    
-    for(int a = 0; a<= windowsize-1; a++)            //Sum the elements in the array
-    {
-        sum1 += StoreArray0[a];
-        sum2 += StoreArray1[a];
-        sum3 += StoreArray2[a];
-    }
-    
-    movAg0 = sum1/windowsize;                        //calculates an average in the array
-    movAg1 = sum2/windowsize;
-    movAg2 = sum3/windowsize;
-}
-
-void emg_filtered()             //Call all filter functions
-{
-    EMGFilter0();
-    EMGFilter1();
-    EMGFilter2();
-    MovAg();
-}
 
 void switch_to_calibrate()
 {
-    x++;
-    
+    x++;                        //Every time function gets called, x increases. Every button press --> new calibration state.
+                                //Starts with x = -1. So when function gets called 1 time, x = 0.  In the end, x = 4 will reset to -1.
+
     if(x==0)                    //If x = 0, led is red
     {
         ledr = 0;
@@ -165,20 +94,20 @@
         ledb = 0;
         ledg = 1;
     }
-    else if (x == 2)            //If x = 2, led is green
+    else if (x==2)            //If x = 2, led is green
     {
         ledr = 1;
         ledb = 1;
         ledg = 0;
     }
-    else                        //If x = 3, led is white
+    else                        //If x = 3 or 4, led is white
     {
         ledr = 0;
         ledb = 0;
         ledg = 0;
     }
    
-    if(x==4)                    //Reset back to x = 0
+    if(x==4)                    //Reset back to x = -1
     {
         x = -1;
     }
@@ -189,25 +118,25 @@
 {
     switch(x)
     {
-        case 0: 
+        case 0:                                         //If calibration state 0:
         {
             sum = 0.0;
-            for(int j = 0; j<=sizeCal-1; j++)
+            for(int j = 0; j<=sizeCal-1; j++)           //Array filled with datapoints from the EMGfilter signal of muscle 0
             {
-                StoreCal0[j] = emgfilter0;
+                StoreCal0[j] = emg0_filt;
                 sum+=StoreCal0[j];
-                wait(0.001f);
+                wait(0.001f);                           //Does there need to be a wait?
             }
-            Mean0       = sum/sizeCal;
-            Threshold0  = Mean0/2;
-            break;
+            Mean0       = sum/sizeCal;                  //Calculate mean of the datapoints in the calibration set (2000 samples)
+            Threshold0  = Mean0/2;                      //Threshold calculation = 0.5*mean
+            break;                                      //Stop. Threshold is calculated, we will use this further in the code
         }
-        case 1:
+        case 1:                                         //If calibration state 1:
         {
-            sum = 0.0;
-            for(int j = 0; j<=sizeCal-1; j++)
+            sum = 0.0;                                  
+            for(int j = 0; j<=sizeCal-1; j++)           //Array filled with datapoints from the EMGfilter signal of muscle 1
             {
-                StoreCal1[j] = emgfilter1;
+                StoreCal1[j] = emg1_filt;
                 sum+=StoreCal1[j];
                 wait(0.001f);
             }
@@ -215,12 +144,12 @@
             Threshold1  = Mean1/2;
             break;
         }
-        case 2:
+        case 2:                                         //If calibration state 2:
         {
             sum = 0.0;
-            for(int j = 0; j<=sizeCal-1; j++)
+            for(int j = 0; j<=sizeCal-1; j++)           //Array filled with datapoints from the EMGfilter signal of muscle 2
             {
-                StoreCal1[j] = emgfilter2;
+                StoreCal1[j] = emg2_filt;
                 sum+=StoreCal2[j];
                 wait(0.001f);
             }
@@ -228,148 +157,179 @@
             Threshold2  = Mean2/2;
             break;
         }
-        case 3:                                     //EMG is calibrated, robot can be set to Home position.
+        case 3:                                         //EMG is calibrated, robot can be set to Home position.
         {
-            emg_cal = 1;
+            emg_cal = 1;                                //This is the setting for which the motors can begin turning in this code (!!)
             wait(0.001f);
             break;
         }
-        default:                                    //Ensures nothing happens if x is not 0,1 or 2.
+        default:                                        //Ensures nothing happens if x is not 0,1 or 2.
         {
             break;
         }
     }
 }
-            
-void encoderA_rise()       
+
+void HIDScope_sample()
+{    
+    /* 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,emg0_raw);
+    //scope.set(1,emg0_filt);
+    scope.set(1,movAg0);          //als moving average werkt
+    scope.set(2,emg1_raw);
+    //scope.set(3,emg1_filt);
+    scope.set(3,movAg1);          //als moving average werkt
+    scope.set(4,emg2_raw);
+    //scope.set(5,emg2_filt);
+    scope.set(5,movAg2);          //als moving average werkt
+
+    scope.send();                   //Send data to HIDScope server
+}
+
+void EMGFilter0()
+{   
+    emg0_raw      = emg0_in.read();                      //give name to raw EMG0 data
+    emg0_filt_x   = emg0filter.step(emg0_raw);           //Use biquad chain to filter raw EMG data
+    emg0_filt     = abs(emg0_filt_x);                    //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
+}
+
+void EMGFilter1()
 {
-    if(encoderB==false)
-    {
-        encoder++;
-    }
-    else
-    {
-        encoder--;
-    }
+    emg1_raw      = emg1_in.read();                      //give name to raw EMG1 data
+    emg1_filt_x   = emg1filter.step(emg1_raw);           //Use biquad chain to filter raw EMG data
+    emg1_filt     = abs(emg1_filt_x);                    //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
 }
 
-void encoderA_fall()      
+void EMGFilter2()
 {
-    if(encoderB==true)
+    emg2_raw      = emg2_in.read();                      //Give name to raw EMG1 data
+    emg2_filt_x   = emg2filter.step(emg2_raw);           //Use biquad chain to filter raw EMG data
+    emg2_filt     = abs(emg2_filt_x);                    //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
+}
+ 
+void MovAg()                                         //Calculate moving average (MovAg), klopt nog niet!!
+{
+    for (int i = windowsize-1; i>=0; i--)            //Make arrays for the last datapoints of the filtered signals
     {
-        encoder++;
+        StoreArray0[i] = StoreArray0[i-1];           //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
+        StoreArray1[i] = StoreArray1[i-1];
+        StoreArray2[i] = StoreArray2[i-1];
     }
-    else
+    
+    StoreArray0[0] = emg0_filt;                      //Stores the latest datapoint of the filtered signal in the first element of the array
+    StoreArray1[0] = emg1_filt;
+    StoreArray2[0] = emg2_filt;
+    
+    sum1 = 0.0;
+    sum2 = 0.0;
+    sum3 = 0.0;
+    
+    for(int a = 0; a<= windowsize-1; a++)            //Sums the elements in the arrays
     {
-        encoder--;
+        sum1 += StoreArray0[a];
+        sum2 += StoreArray1[a];
+        sum3 += StoreArray2[a];
     }
+    
+    movAg0 = sum1/windowsize;                        //calculates an average in the array
+    movAg1 = sum2/windowsize;
+    movAg2 = sum3/windowsize;
+    //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
 }
 
-void encoderB_rise()       
+void emg_filtered()             //Call all filter functions
 {
-    if(encoderA==true)
-    {
-        encoder++;
-    }
-    else
-    {
-        encoder--;
-    }
-}
-
-void encoderB_fall()      
-{
-    if(encoderA==false)
-    {
-        encoder++;
-    }
-    else
-    {
-        encoder--;
-    }
+    EMGFilter0();
+    EMGFilter1();
+    EMGFilter2();
 }
 
 
-// Main function start.
+int main()
+{         
+        //pc.baud(115200);
+        //pc.printf("Hello World!\r\n");                                                          //Serial communication only works if hidscope is turned off.
+        
+        emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 );        //attach biquad elements to chain
+        emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
+        emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
 
-int main()
-{ 
-    //pc.baud(115200);
-    //pc.printf("hello\n\r");
+        filter_tick.attach(&emg_filtered,T);        //EMG signals filtered every T sec.
+        MovAg_tick.attach(&MovAg,T);                //Moving average calculation every T sec.
+        HIDScope_tick.attach(&HIDScope_sample,T);   //EMG signals raw + filtered to HIDScope every T sec.
     
-    ledr = 0;       //Begin led = red, first state of calibration
-    ledb = 1;
+    ledr = 1;                                   //Begin led = blue, press button for first state of calibration --> led will turn red
+    ledb = 0;
     ledg = 1;
-    
-    sample_timer.attach(&sample, 0.002);        //HIDscope 
-    
-    filter_tick.attach(&emg_filtered,T);        //EMG signals filtered + moving average every T sec.
+        
     button1.rise(switch_to_calibrate);          //Switch state of calibration (which muscle)
     wait(0.2f);
-    button2.rise(calibrate);                    //calibrate threshold for 3 muscles
+    button2.rise(calibrate);                    //Calibrate threshold for 3 muscles
     wait(0.2f);
        
     pwmpin1.period_us(60);                      //60 microseconds PWM period, 16.7 kHz 
-
-    encoderA.rise(&encoderA_rise);
-    encoderA.fall(&encoderA_fall);
-    encoderB.rise(&encoderB_rise);
-    encoderB.fall(&encoderB_fall);
     
-    if(emg_cal==1)
+    if(emg_cal==1)                              //After calibration is finished, emg_cal will be 1. Otherwise 0. 
     { 
-         while (true)
-                {
-                //Motor aansturen en encoder uitlezen
-                  //float u1 = potmetervalue1;
-                  //float u2 = potmetervalue2;
-                  
-                  //float m1 = ((u1*2.0f)-1.0f);
-                  //float m2 = ((u2*2.0f)-1.0f);
-                
-                  //pwmpin1 = fabs(m1*0.6f)+0.4f;     //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.        
-                
-                if(emgfilter0>Threshold0)
+         //while (true)
+           // {
+               
+                if(movAg0>Threshold0)        //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
                 {
                        pwmpin1 = 1;
                        directionpin1.write(1);
+                       
+                       ledr = 1;                                   //Blue
+                       ledb = 0;
+                       ledg = 1;
+                       
                 }
-                else
+                else                            //If it is not higher than the threshold, the motor will not turn at all. 
                 {
                        pwmpin1 = 0;
+                       ledr = 0;                                   //white
+                       ledb = 0;
+                       ledg = 0;
                 }
                 
-                if(emgfilter1>Threshold1)
+                if(movAg1>Threshold1)        //If the filtered EMG signal of muscle 1 is higher than the threshold, motor2 will turn in 1 direction
                 {
                        pwmpin2 = 1;
                        directionpin2.write(1);
+                       ledr = 0;                                   //red
+                       ledb = 1;
+                       ledg = 1;
                 }
-                else
+                else                            //If not higher than the threshold, motor will not turn at all
                 {
                        pwmpin2 = 0;
+                       ledr = 0;                                   //white
+                       ledb = 0;
+                       ledg = 0;
                 }
-                if(emgfilter2>Threshold2)
+                if(movAg2>Threshold2)        //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
                 {
                        pwmpin1 = 1;
                        pwmpin2 = 2;
                        directionpin1.write(1);
                        directionpin2.write(1);
+                       
+                       ledr = 1;                                   //green
+                       ledb = 1;
+                       ledg = 0;
                 }
-                else
+                else                            //If not higher than the threshold, motors will not turn at all
                 {
                        pwmpin1 = 0;
                        pwmpin2 = 0;
+                       
+                       ledr = 0;                                   //white
+                       ledb = 0;
+                       ledg = 0;
                 }
                        
-                       //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. 
-                //wait(0.01f);                   //zodat de code niet oneindig doorgaat.
-                //pwmpin2 = fabs(m2*0.6f)+0.4f;    
-                //directionpin2.write(m2>0);   
-                
-                //float encoderDegrees = float(encoder)*(360.0/8400.0);
-                        
-                //pc.printf("Encoder count: %f \n\r",encoderDegrees);
+                    
         
-    }
+            //}
+    }   
 }
-}