Diff: main.cpp

Revision:

9:d4927ec5862f

Parent:

8:9edf32e021a5

Child:

10:39a97906fa4b

--- a/main.cpp	Mon Oct 23 09:22:38 2017 +0000
+++ b/main.cpp	Mon Oct 23 10:06:37 2017 +0000
@@ -9,8 +9,9 @@
 MODSERIAL pc(USBTX,USBRX);
 
 // ---- EMG parameters ----
-//HIDScope scope (2); 
+HIDScope scope (2); 
 EMG_filter emg1(A0);
+float emgthreshold = 0.20;
 // ------------------------ 
 
 // ---- Motor input and outputs ----
@@ -91,28 +92,19 @@
     if (go_calibration)
     {
         led2 = 1;
-        //emg1.calibration();                 // Using the calibration function of the EMG_filter class              
+        emg1.calibration();                 // Using the calibration function of the EMG_filter class              
     }
 }
 
 // ------------------------------
 
-void setpointreadout()
-{
-    
-    potvalue = pot.read();
-    setpoint = potvalue*6.28f;
-    
-    potvalue2 = pot2.read();
-    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();
@@ -120,7 +112,21 @@
         
         scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope  
         scope.set(1, emg1.emg0);        
-        scope.send();*/
+        scope.send();
+   
+}
+
+void setpointreadout()
+{
+    
+    potvalue = pot.read();
+    if (emg1.normalized>=emgthreshold)
+    {
+    setpoint = emg1.normalized*6.50;                //setpoint in translational direction (cm). EMG output (0.2-1.0) scaled to maximal translation distance (52 cm). 
+    }
+    
+    potvalue2 = pot2.read();
+    setpoint2 = potvalue2*6.28f;
    
 }
  
@@ -129,7 +135,7 @@
 void PIDcalculation() // inputs: potvalue, motor#, setpoint
 {
     setpointreadout();
-    angle = motor1.getPosition()/4200.00*6.28;   
+    angle = motor1.getPosition()/4200.00;   
     angle2 = motor2.getPosition()/4200.00*6.28;
    
     //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
@@ -218,7 +224,7 @@
         if(count == 100){
             count = 0;
             pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint);
-            pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
+           // pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
         }