Diff: main.cpp

Revision:

42:e67627d11789

Parent:

41:a666a531d52e

Child:

43:42bfab67c4a5

--- a/main.cpp	Wed Jun 03 14:13:32 2015 +0000
+++ b/main.cpp	Wed Jun 03 14:26:56 2015 +0000
@@ -108,6 +108,7 @@
     damping1 = speed1 * Damp;
     step_freq1 = (setpoint*speed1);
     speed_old1 = speed1;
+    
     //Achteruit
     force2 = K_Gain*(filtered_triceps/MAX_tri);
     force2 = force2 - damping2;
@@ -119,20 +120,21 @@
 
     if (filtered_biceps > filtered_triceps) {
         Dir = 0;
+        step_freq2 = 1;
         Step.period(1.0/step_freq1);
-
-    } else {
+    } if (filtered_triceps > filtered_biceps) {
         Dir = 1;
+        step_freq1 = 1; 
         Step.period(1.0/step_freq2);
     }
     //Speed limit
-    if (speed1 > 1 || speed2 > 1) {
+    if (speed1 > 1) {
         speed1 = 1;
+        step_freq1 = setpoint;
+    }
+    if (speed2 > 2) {
         speed2 = 1;
-        Step.period(1.0/setpoint);
-    } else {
-        step_freq1 = (setpoint*speed1);
-        step_freq2 = (setpoint*speed2);
+        step_freq1 = setpoint;
     }
     //EMG treshold
     if (filtered_biceps < EMG_tresh && filtered_triceps < EMG_tresh) {