Diff: main.cpp

Revision:

7:809f122886ae

Parent:

6:fc46581fe3e0

--- a/main.cpp	Mon Oct 23 07:48:56 2017 +0000
+++ b/main.cpp	Mon Oct 23 08:26:02 2017 +0000
@@ -3,9 +3,16 @@
 #include "encoder.h"
 #include "math.h"
 #include "HIDScope.h"
+#include "BiQuad.h"
+#include "EMG_filter.h"
 
-//HIDScope scope(1);
+
+
 MODSERIAL pc(USBTX,USBRX);
+// -------------------------------
+HIDScope scope(2);      // define EMG input and HIDScope output
+EMG_filter emg1(A0);
+//---------------------------------
 PwmOut speed1(D5);
 PwmOut speed2(D6);
 DigitalOut dir1(D4);
@@ -15,7 +22,8 @@
 DigitalOut led2(D11);
 AnalogIn pot(A2);
 AnalogIn pot2(A1);
-Ticker mainticker;
+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.
 Encoder motor1(PTD0,PTC4);
 Encoder motor2(D12,D13);
 
@@ -52,22 +60,62 @@
 volatile double position = 0.0;
 volatile double position2 = 0.0;
 
-//bool readoutsetpoint = true;
+// ---- variables for the ticker ----
+//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     
+
 
 void setpointreadout()
-{
-   
+{  
     potvalue = pot.read();
     setpoint = potvalue*6.28f;
     
     potvalue2 = pot2.read();
-    setpoint2 = potvalue2*6.28f;
+    setpoint2 = potvalue2*6.28f;   
+}
+/*
+//Function that reads out the raw EMG and filters the signal 
+void processEMG ()
+{
+    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();
+    }
    
+} 
+
+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 PIDcalculation() // inputs: potvalue, motor#, setpoint
 {
+    
     setpointreadout();
     angle = motor1.getPosition()/4200.00*6.28;   
     angle2 = motor2.getPosition()/4200.00*6.28;
@@ -128,9 +176,14 @@
     speed2 = pidTerm_scaled2+0.45f;
 }
 
+void maintickerfunction()
+{  
+    PIDcalculation();
+}
+
 int main()
 {
-    mainticker.attach(PIDcalculation,0.01f);
+    mainticker.attach(&maintickerfunction,0.01f);
     speed1.period(PwmPeriod);
     speed2.period(PwmPeriod);