Diff: main.cpp

Revision:

0:f49bda586fc5

Child:

1:90041f90ecf2

Child:

2:810de22bca6a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 26 10:09:33 2017 +0000
@@ -0,0 +1,165 @@
+#include "mbed.h"
+#include "math.h"
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+#include "BiQuad.h"
+#include "EMG_filter.h"
+
+MODSERIAL pc(USBTX,USBRX);
+//HIDScope scope (2);
+
+enum r_states {R_HORIZONTAL, R_VERTICAL, R_BASE};
+r_states state;
+
+//Define EMG inputs
+EMG_filter emg1(A0);
+EMG_filter emg2(A1);
+DigitalOut ledred(LED_RED);
+DigitalOut ledgreen(LED_GREEN);
+DigitalOut ledblue(LED_BLUE);
+DigitalOut ledstateswitch(D8);
+DigitalOut ledstatedef(D11);
+
+
+//EMG_filter emg3(A2);
+
+//Define ticker1
+Ticker mainticker;      //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
+Timeout calibrationgo;      // Timeout that will determine the duration of the calibration.
+
+//Define ticker variables
+bool go_EMG;                    // Boolean to put on/off the EMG readout
+bool go_calibration;            // Boolean to put on/off the calibration of the EMG
+bool go_switch;
+
+//Function that reads out the raw EMG and filters the signal
+
+
+void calibrationGO()                   // Function for go or no go of calibration
+{
+    go_calibration = false;
+}
+
+/*
+Calibration of the robot works as follows:
+EMG signal is read out for 5 seconds, 1000 times a second. This signal is filtered and the maximum value is compared to the current value of the MVC.
+The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
+During the calibration the user should exert maximum force.
+*/
+
+void calibrationEMG()                   // Function for calibrating the EMG signal
+{
+    if (go_calibration) {
+        emg1.calibration();                 // Using the calibration function of the EMG_filter class
+    }
+}
+
+
+void r_movehorizontal()
+{
+
+
+}
+
+void r_movevertical()
+{
+
+}
+
+void r_movebase()
+{
+
+}
+
+void r_processStateSwitch()
+{
+    while(go_switch) {
+        switch(state) {
+            case R_HORIZONTAL:
+                state = R_VERTICAL;
+                ledblue = 1;
+                ledred = 1;
+                ledgreen = 0;
+                pc.printf("state is vertical");
+                break;
+            case R_VERTICAL:
+                state = R_BASE;
+                ledgreen = 1;
+                ledblue = 1;
+                ledred = 0;
+                pc.printf("state is base");
+                break;
+            case R_BASE:
+                state = R_HORIZONTAL;
+                ledgreen = 1;
+                ledred = 1;
+                ledblue = 0;
+                pc.printf("state is horizontal");
+                break;
+                }
+                wait(3.0f);
+                go_switch = false;
+                ledstatedef = 1;
+                ledstateswitch = 0;
+        }   
+}
+
+void processEMG ()
+{
+    r_processStateSwitch();
+    if (go_EMG) {
+        //go_EMG = false;                   //set the variable to false --> misschien nodig?
+        emg1.filter();                      //filter the incoming emg signal
+        emg2.filter();
+        //emg3.filter();
+        /*
+        scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope
+        scope.set(1, emg1.emg0);
+        scope.send();*/
+    }
+
+}
+
+
+
+int main()
+{
+    pc.baud(115200);                    // Set baudrate for proper communication
+    go_EMG = true;                      // Setting ticker variables
+    go_calibration = true;              // Setting the timeout variable
+    calibrationgo.attach(&calibrationGO, 5.0);     // Attach calibration timeout to calibration function
+    mainticker.attach(&calibrationEMG, 0.001f);      // Attach ticker to the calibration function
+    wait(5.0);                                         // Wait for 5.0 seconds to make time for calibration
+    mainticker.attach(&processEMG, 0.001f);         // Attaching mainticker to function processEMG, calling  the function 1000 times a second
+
+    while (true) {
+        ledstatedef = 1;
+        if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
+            ledstateswitch = 1;
+            ledstatedef = 0;
+            go_switch = true;
+
+        }
+
+
+        switch(state) {
+            case R_HORIZONTAL:
+                r_movehorizontal();
+                break;
+            case R_VERTICAL:
+                r_movevertical();
+                break;
+            case R_BASE:
+                r_movebase();
+                break;
+                       }
+
+
+    }
+}
+
+
+
+
+
+