Diff: main.cpp

Revision:

7:d4090f334ce2

Parent:

6:a02ad75f0333

Child:

8:2afb66572fc4

--- a/main.cpp	Thu Oct 25 13:55:32 2018 +0000
+++ b/main.cpp	Mon Oct 29 13:19:43 2018 +0000
@@ -4,6 +4,13 @@
 #include "FastPWM.h"
 #include "math.h"
 
+#include "mbed.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
+#include "BiQuad4.h"
+#include "FilterDesign.h"
+#include "FilterDesign2.h"
+
 // LED's
 DigitalOut led_red(LED_RED);
 DigitalOut led_green(LED_GREEN);
@@ -21,10 +28,19 @@
 DigitalOut directionM2(D7);
 FastPWM motor1_pwm(D5);
 FastPWM motor2_pwm(D6);
+// EMG input en start value of filtered EMG
+AnalogIn    emg1_raw( A0 );
+AnalogIn    emg2_raw( A1 );
+double emg1_filtered = 0.00;
+double emg2_filtered = 0.00;
+float threshold_EMG = 0.25;         // Threshold on 25 percent of the maximum EMG
+
 // Declare timers and Tickers
-Timer timer;                        // Timer for counting time in this state
-Ticker WriteValues;                 // Ticker to show values of velocity to screen
-Ticker StateMachine;
+Timer       timer;                        // Timer for counting time in this state
+Ticker      WriteValues;            // Ticker to show values of velocity to screen
+Ticker      StateMachine;
+Ticker      sample_EMGtoHIDscope;   // Ticker to send the EMG signals to screen
+HIDScope    scope(4);               //Number of channels which needs to be send to the HIDScope
 
 // Set up ProcessStateMachine
 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE};
@@ -32,7 +48,6 @@
 bool stateChanged = true;
 volatile bool writeVelocityFlag = false;
 
-
 // Global variables
 char c;
 int counts1; 
@@ -47,6 +62,11 @@
 float tijd = 0.005;
 float time_in_state;
 
+int need_to_move_1;                     // Does the robot needs to move in the first direction?
+int need_to_move_2;                     // Does the robot needs to move in the second direction?
+double EMG_calibrated_max_1 = 2.00000;  // Maximum value of the first EMG signal found in the calibration state.
+double EMG_calibrated_max_2 = 2.00000;  // Maximum value of the second EMG signal found in the calibration state.
+
 // ----------------------------------------------
 // ------- FUNCTIONS ----------------------------
 // ----------------------------------------------
@@ -78,6 +98,17 @@
         directionM1 = U1 > 0.0f;            // Either true or false, determines direction.
         directionM2 = U2 > 0.0f;
 } 
+void sample()
+{
+    emg1_filtered = FilterDesign(emg1_raw.read());
+    emg2_filtered = FilterDesign2(emg2_raw.read());
+    
+    scope.set(0, emg1_raw.read());      // Raw EMG 1 send to scope 0
+    scope.set(1, emg1_filtered);        // Filtered EMG 1 send to scope 1
+    scope.set(2, emg2_raw.read());      // Raw EMG 2 send to scope 2
+    scope.set(3, emg2_filtered);        // Filtered EMG 2 send to scope 3
+    scope.send();                       // Send the data to the computer
+}
 // ---------------------------------------------------
 // --------STATEMACHINE------------------------------- 
 // --------------------------------------------------- 
@@ -262,12 +293,24 @@
                     
         led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
         pc.printf("MOVE_W_EMG, Moving with use of EMG \r\n");
-                
+             
+        if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
+            need_to_move_1 = 1; // The robot does have to move
+            }
+        else {
+            need_to_move_1 = 0; // If the robot does not have to move
+            }
+
+        if(emg2_filtered >= threshold*EMG_calibrated_max_2){
+            need_to_move_2 = 1;
+            }
+        else {
+            need_to_move_2 = 0;
+            }   
         // Requirements to move to the next state:
         // When the home button or the failure button is pressed, we 
         // will the move to the corresponding state.
                 
-        // BUILD THE MOVEMENT WITH EMG
         wait(1);
         c = pc.getcNb();
         if (c == 'h') 
@@ -339,7 +382,7 @@
     
     pc.baud(115200);
     StateMachine.attach(&ProcessStateMachine, 0.005f);   // Run statemachine 200 times per second
-  
+    sample_EMGtoHIDscope.attach(&sample, 0.02f);         // Display EMG values 50 times per second
     while (true) {
         
     }