Revision 10:39a97906fa4b, committed 2017-10-23

Comitter:

EvB

Date:

Mon Oct 23 12:48:38 2017 +0000

Parent:

9:d4927ec5862f

Commit message:

Probeersel met overschrijven referentiepositie

Changed in this revision

--- a/main.cpp	Mon Oct 23 10:06:37 2017 +0000
+++ b/main.cpp	Mon Oct 23 12:48:38 2017 +0000
@@ -12,6 +12,7 @@
 HIDScope scope (2); 
 EMG_filter emg1(A0);
 float emgthreshold = 0.20;
+double MVC = 0.0;
 // ------------------------ 
 
 // ---- Motor input and outputs ----
@@ -37,12 +38,13 @@
 float PwmPeriod = 0.0001f;
 
 double count = 0; //set the counts of the encoder
+double countmax = 0; //counter to find max peak of EMG signal every second
 volatile double angle = 0;//set the angles
 
 double count2 = 0; //set the counts of the encoder
 volatile double angle2 = 0;//set the angles
 
-double setpoint = 6.28;//I am setting it to move through 180 degrees
+double setpoint;//I am setting it to move through 180 degrees
 double setpoint2 = 6.28;//I am setting it to move through 180 degrees
 double Kp = 250;// you can set these constants however you like depending on trial & error
 double Ki = 100;
@@ -116,25 +118,37 @@
    
 }
 
-void setpointreadout()
+double peakdetection ()
+{
+    
+    if (emg1.normalized>=MVC)
+    {
+        MVC = emg1.normalized; 
+    }
+    countmax++;
+    if(countmax==100 && MVC>=emgthreshold)
+    {
+        setpoint = MVC*6.50;                         //setpoint in translational direction (cm). EMG output (0.2-1.0) scaled to maximal translation distance (52 cm). 
+        countmax = 0;
+        MVC = 0;
+    }
+    return setpoint; 
+}
+
+/*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;
    
-}
+}*/
  
    
 
 void PIDcalculation() // inputs: potvalue, motor#, setpoint
 {
-    setpointreadout();
+    setpoint = peakdetection();
     angle = motor1.getPosition()/4200.00;   
     angle2 = motor2.getPosition()/4200.00*6.28;
    
@@ -223,7 +237,7 @@
         count++;
         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 = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f, MVC = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint,MVC);
            // 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);
         }