Revision 39:191ae0d12bd6, committed 2015-06-01

Comitter:

jessekaiser

Date:

Mon Jun 01 11:34:57 2015 +0000

Parent:

38:c592354f5080

Child:

40:0cfd96cb25fa

Commit message:

Poging 2, werkt waarschijnlijk slechter;

Changed in this revision

--- a/main.cpp	Mon Jun 01 11:25:19 2015 +0000
+++ b/main.cpp	Mon Jun 01 11:34:57 2015 +0000
@@ -60,11 +60,13 @@
 //global variabels
 float filtered_biceps;
 float filtered_triceps;
-float speed_old;
+float speed_old1;
+float speed_old2;
 float acc;
 float force1;
 float force2;
-float speed;
+float speed1;
+float speed2; 
 float D;
 
 void looper_emg()
@@ -97,33 +99,40 @@
     
         if (filtered_biceps > filtered_triceps) {
             Dir = 0;
-            force2 = 0;
+            //force2 = 0;
+            speed2 = 0;
             force1 = K_Gain*(filtered_biceps/MAX_bi);
             force1 = force1 - D;
             acc = force1/Mass;
-            speed = speed_old + (acc * dt);
-            D = speed * Damp;
-    
+            speed1 = speed_old1 + (acc * dt);
+            D = speed1 * Damp;
+            step_freq = (setpoint*speed1);
+            Step.period(1.0/step_freq);
+            speed_old1 = speed1;
         } else {
             Dir = 1;
-            force1 = 0;
+            //force1 = 0;
+            speed1 = 0;
             force2 = K_Gain*(filtered_triceps/MAX_tri);
             force2 = force2 - D;
             acc = force2/Mass;
-            speed = speed_old + (acc * dt);
-            D = speed * Damp;
+            speed2 = speed_old2 + (acc * dt);
+            D = speed2 * Damp;
+            step_freq = (setpoint*speed2);
+            Step.period(1.0/step_freq);
+            speed_old2 = speed2;
            
             
         }
         //Speed limit
-        if (speed > 1) {
+        /*if (speed > 1) {
             speed = 1;
             step_freq = setpoint;
         } else {
             step_freq = (setpoint*speed);
         }
         Step.period(1.0/step_freq);
-        speed_old = speed;
+        speed_old = speed;*/
         
 
         if (step_freq < MIN_freq) {
@@ -155,7 +164,7 @@
 
         while (1) {
 
-            lcd.printf("Freq %.0f Hz, %.2f \n", step_freq, speed); //snelheid meting op lcd
+            lcd.printf("Freq %.0f Hz \n", step_freq); //snelheid meting op lcd
             //pc.printf("%.3f \n", emg0.read());
             wait(0.01);
         }