Diff: main.cpp

Revision:

0:5c4ee2c81f02

Child:

2:7e0279519cbf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 12 10:19:08 2017 +0000
@@ -0,0 +1,94 @@
+#include "mbed.h"
+#include "Serial.h"
+#include "math.h"
+#include "HIDScope.h"
+
+Serial      pc(USBTX, USBRX);        //Serial PC connectie
+AnalogIn    emg0( A0 );              //EMG1 op A0
+AnalogIn    emg1( A1 );              //EMG2 op A1
+DigitalOut  motor1DirectionPin(D4);  //Motorrichting op D4 (connected op het bord)
+PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
+Timer       timer;                   //timer voor duur script        
+HIDScope    scope(2);                //Maakt de scopes aan   
+
+//Benoemen van de variabelen die in de VOID's gebruikt gaan worden
+double emga = emg0.read();          //EMG1
+double emgb = emg1.read();          //EMG2
+
+//Aanmaken filter variabelen
+float ah[3]={1, 0, 0.1716};
+float bh[3]={0.2929, -0.5858, 0.2929};
+
+//innitial conditions high pass filter
+float emg_hpf[3]={0, 0, 0};
+float emg_in[3]={0, 0, 0};
+
+// coëfficienten high pass filter
+float al[3]={1, -1.7347, 0.7660};
+float bl[3]={0.0078, 0.0156, 0.0078};
+
+//innitial conditions low pass filter
+float emg_lpf[3]={0, 0, 0};
+float emg_abs[3]={0, 0, 0};
+
+//Aanmaken van de verschillende tickers
+Ticker tick_sample;
+
+void aansturing()
+    {
+    timer.reset();
+    timer.start();  
+
+        emga = emg0.read();
+        emgb = emg1.read();
+        emg_in[0]=emga;
+        
+            //Filter
+                    // high pass filter
+                    emg_hpf[0]=bh[0]*emg_in[0]    +bh[1]*emg_in[1]  +bh[2]*emg_in[2]  -ah[1]*emg_hpf[1]  -ah[2]*emg_hpf[2];
+        
+                    emg_in[2]=emg_in[1];
+                    emg_in[1]=emg_in[0];
+                    emg_hpf[2]=emg_hpf[1];
+                    emg_hpf[2]=emg_hpf[0];
+        
+
+                    //absolute value
+                    emg_abs[0]=fabs(emg_hpf[0]);
+        
+        
+                    //low pass filter
+                    emg_lpf[0]=bl[0]*emg_abs[0]   +bl[1]*emg_abs[1] +bl[2]*emg_abs[2] -al[1]*emg_lpf[1] -al[2]*emg_lpf[2];
+        
+                    emg_abs[2]=emg_abs[1];
+                    emg_abs[1]=emg_abs[0];
+                    emg_lpf[2]=emg_lpf[1];
+                    emg_lpf[1]=emg_lpf[0];
+                
+        if (emg_lpf[1]>0.05)
+        {
+        motor1MagnitudePin = emg_lpf[1];
+        motor1DirectionPin = 0;
+        }
+        else 
+        {
+        motor1MagnitudePin = 0;
+        motor1DirectionPin = 0;
+        }
+    scope.set(0, emg0.read());
+    scope.send();
+    scope.set(1, emg_lpf[1]);
+    scope.send();
+
+        timer.stop();   
+    pc.printf("time taken was %f milliseconds\n\r", timer.read_ms());
+        
+    }
+
+
+int main()
+{
+    //Deze tickers roepen de verschillende voids aan
+    pc.baud(9600);
+    tick_sample.attach_us(&aansturing, 5000);    //Deze ticker roept de potmeter aan
+}