Diff: main.cpp

Revision:

13:a3d4b4daf5b4

Parent:

12:eaed305a76c3

Child:

14:fa09dae67390

diff -r eaed305a76c3 -r a3d4b4daf5b4 main.cpp
--- a/main.cpp	Fri Oct 19 14:39:35 2018 +0000
+++ b/main.cpp	Mon Oct 22 08:35:21 2018 +0000
@@ -8,12 +8,17 @@
 AnalogIn emg1_in            (A1);
 AnalogIn emg2_in            (A2);
 
-DigitalIn button2           (D10);                  //Let op, is deze niet bezet?
+DigitalIn   button1         (D10);                  //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
+DigitalIn   button2         (D11);
 InterruptIn encoderA        (D9);
 InterruptIn encoderB        (D8);
 
 DigitalOut directionpin1    (D4);
 DigitalOut directionpin2    (D7);
+DigitalOut led1             (LED_RED);
+DigitalOut led2             (LED_BLUE);
+DigitalOut led3             (LED_GREEN);
+
 PwmOut pwmpin1              (D5);
 PwmOut pwmpin2              (D6);
 
@@ -22,9 +27,25 @@
 
 
 //Global variables
-int encoder     = 0;
+int encoder     = 0;                   //Starting point encoder
 const float T   = 0.001f;              //Ticker period
 
+//EMG filter
+double emgfilter0, emgfilter1, emgfilter2;                                                       //Filtered EMG data 0, 1 and 2
+double windowsize = 150;                                                                         //Size of the array over which the moving average (MovAg) is calculated
+double sum, sum1, sum2, sum3;                                                                    //variables used to sum elements in array
+double StoreArray0[sizeMovAg] = [], StoreArray1[sizeMoveAg] = [], StoreArray2[sizeMoveAg] = [];  //Empty arrays to calculate MoveAg
+double movAg0,movAg1,movAg2;                                                                     //outcome of MovAg
+
+//calibration
+int x = 0;
+int emg_cal = 0;
+const int sizeCal = 2000;
+double storeCal0[sizeCal] = [], storeCal1[sizeCal] = [], storeCal2[sizeCal] = [];
+double meanCal0,meanCal1,meanCal2;
+double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1;
+
+
 //Biquad
 BiQuadChain emg0band;
 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
@@ -73,18 +94,135 @@
     double notch2        = notch1.step(absolute2);
 }
 
-void emg_filtered()             //call all filter functions
+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/sizeMovAg;
+}
+
+void emg_filtered()             //Call all filter functions
 {
     EMGFilter0();
     EMGFilter1();
     EMGFilter2();
+    MovAg();
 }
 
-void MovAg()                    //calculate moving average
+void switch_to_calibrate()
+{
+    x++;
+    
+    if(x==0)                    //If x = 0, led is red
+    {
+        led1 = 0;
+        led2 = 1;
+        led3 = 1;
+    }
+    else if (x==1)              //If x = 1, led is blue
+    {
+        led1 = 1;
+        led2 = 0;
+        led3 = 1;
+    }
+    else if (x == 2)            //If x = 2, led is green
+    {
+        led1 = 1;
+        led2 = 1;
+        led3 = 0;
+    }
+    else                        //If x > 3, led is white
+    {
+        led1 = 0;
+        led2 = 0;
+        led3 = 0;
+    }
+   
+    if(x>=4)                    //Reset back to x = 0
+    {
+        x = 0;
+    }
+}
+    
+        
+void calibrate(void)
 {
-    for i = 
+    switch(x)
+    {
+        case 0: 
+        {
+            sum = 0.0;
+            for(int j = 0; j<=sizeCal-1; j++)
+            {
+                StoreCal0[j] = emgfilter0;
+                sum+=StoreCal0[j];
+                wait(0.001f);
+            }
+            Mean0       = sum/sizeCal;
+            Threshold0  = Mean0/2;
+            break;
+        }
+        case 1:
+        {
+            sum = 0.0;
+            for(int j = 0; j<=sizeCal-1; j++)
+            {
+                StoreCal1[j] = emgfilter1;
+                sum+=StoreCal1[j];
+                wait(0.001f);
+            }
+            Mean1       = sum/sizeCal;
+            Threshold1  = Mean1/2;
+            break;
+        }
+        case 2:
+        {
+            sum = 0.0;
+            for(int j = 0; j<=sizeCal-1; j++)
+            {
+                StoreCal1[j] = emgfilter2;
+                sum+=StoreCal2[j];
+                wait(0.001f);
+            }
+            Mean2       = sum/sizeCal;
+            Threshold2  = Mean2/2;
+            break;
+        }
+        case 3:                                     //EMG is calibrated, robot can be set to Home position.
+        {
+            emg_cal = 1;
+            wait(0.001f);
+            break;
+        }
+        default:                                    //Ensures nothing happens if x is not 0,1 or 2.
+        {
+            break;
+        }
+    }
 }
-
+            
 void encoderA_rise()       
 {
     if(encoderB==false)
@@ -138,18 +276,18 @@
 
 int main()
 { 
-    pc.baud(115200);
-    pc.printf("hello\n\r");
+    //pc.baud(115200);
+    //pc.printf("hello\n\r");
     
-    //EMG signaal filteren 
-        
-        filter_tick.attach(&emg_filtered,T);        //EMG signals filtered every T sec.
-        MovAg_tick.attach(&MovAg,T);                //Moving average calculation every T sec.
-        
-        while(true){/*do not return from main()*/}
-        
-        
-    pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz 
+    led1 = 0;       //Begin led = red, first state of calibration
+    led2 = 1;
+    led3 = 1;
+    
+    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)
+    button2.rise(calibrate);                    //calibrate threshold for 3 muscles
+            
+    pwmpin1.period_us(60);                      //60 microseconds PWM period, 16.7 kHz 
 
     encoderA.rise(&encoderA_rise);
     encoderA.fall(&encoderA_fall);
@@ -159,21 +297,54 @@
     while (true)
     {
         //Motor aansturen en encoder uitlezen
-          float u1 = potmetervalue1;
-          float u2 = potmetervalue2;
+          //float u1 = potmetervalue1;
+          //float u2 = potmetervalue2;
           
-          float m1 = ((u1*2.0f)-1.0f);
-          float m2 = ((u2*2.0f)-1.0f);
+          //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)
+        {
+               pwmpin1 = 1;
+               directionpin1.write(1);
+        }
+        else
+        {
+               pwmpin1 = 0;
+        }
         
-        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.        
-        directionpin1.write(m1>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);   
+        if(emgfilter1>Threshold1)
+        {
+               pwmpin2 = 1;
+               directionpin2.write(1);
+        }
+        else
+        {
+               pwmpin2 = 0;
+        }
+        if(emgfilter2>Thresheld2)
+        {
+               pwmpin1 = 1;
+               pwmpin2 = 2;
+               directionpin1.write(1);
+               directionpin2.write(1);
+        }
+        else
+        {
+               pwmpin1 = 0;
+               pwmpin2 = 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);
+        //float encoderDegrees = float(encoder)*(360.0/8400.0);
                 
-        pc.printf("Encoder count: %f \n\r",encoderDegrees);
+        //pc.printf("Encoder count: %f \n\r",encoderDegrees);
         
     }
 }