Diff: main.cpp

Revision:

8:9edf32e021a5

Parent:

6:fc46581fe3e0

Child:

9:d4927ec5862f

Child:

11:bda77916d2ea

--- a/main.cpp	Mon Oct 23 07:48:56 2017 +0000
+++ b/main.cpp	Mon Oct 23 09:22:38 2017 +0000
@@ -3,9 +3,17 @@
 #include "encoder.h"
 #include "math.h"
 #include "HIDScope.h"
+#include "BiQuad.h"
+#include "EMG_filter.h"
 
-//HIDScope scope(1);
 MODSERIAL pc(USBTX,USBRX);
+
+// ---- EMG parameters ----
+//HIDScope scope (2); 
+EMG_filter emg1(A0);
+// ------------------------ 
+
+// ---- Motor input and outputs ----
 PwmOut speed1(D5);
 PwmOut speed2(D6);
 DigitalOut dir1(D4);
@@ -15,9 +23,15 @@
 DigitalOut led2(D11);
 AnalogIn pot(A2);
 AnalogIn pot2(A1);
-Ticker mainticker;
 Encoder motor1(PTD0,PTC4);
 Encoder motor2(D12,D13);
+// ----------------------------------
+
+// --- Define Ticker and Timeout ---
+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.
+// ---------------------------------
+
 
 float PwmPeriod = 0.0001f;
 
@@ -52,11 +66,40 @@
 volatile double position = 0.0;
 volatile double position2 = 0.0;
 
+// --- Booleans for the maintickerfunction ---
 //bool readoutsetpoint = true;
+bool go_EMG;                    // Boolean to put on/off the EMG readout
+bool go_calibration;            // Boolean to put on/off the calibration of the EMG   
+// -------------------------------------------
+
+// --- calibrate EMG signal ----
+
+void calibrationGO()                   // Function for go or no go of calibration
+{
+    go_calibration = false;
+    led2 = 0;
+}
+
+/*
+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)
+    {
+        led2 = 1;
+        //emg1.calibration();                 // Using the calibration function of the EMG_filter class              
+    }
+}
+
+// ------------------------------
 
 void setpointreadout()
 {
-   
+    
     potvalue = pot.read();
     setpoint = potvalue*6.28f;
     
@@ -64,7 +107,24 @@
     setpoint2 = potvalue2*6.28f;
    
 }
-    
+
+//Function that reads out the raw EMG and filters the signal 
+void processEMG ()
+{
+        led1 = 1;
+        /*
+        //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();*/
+   
+}
+ 
+   
 
 void PIDcalculation() // inputs: potvalue, motor#, setpoint
 {
@@ -128,12 +188,29 @@
     speed2 = pidTerm_scaled2+0.45f;
 }
 
+void maintickerfunction()
+{     
+    if(go_EMG)
+    {
+        processEMG();
+    }
+    
+    PIDcalculation();    
+}
+
 int main()
-{
-    mainticker.attach(PIDcalculation,0.01f);
+{   
+    go_EMG = true;                      // Setting ticker variables    
+    go_calibration = true;              // Setting the timeout variable
     speed1.period(PwmPeriod);
     speed2.period(PwmPeriod);
     
+    calibrationgo.attach(&calibrationGO, 5.0);     // Attach calibration timeout to calibration function
+    mainticker.attach(&calibrationEMG, 0.001f);      // Attach ticker to the calibration function
+    wait(5.0f);
+    
+    mainticker.attach(&maintickerfunction,0.001f);        
+    
     int count = 0;    
     while(true) {