Diff: main.cpp

Revision:

8:62e968f78878

Parent:

7:1f88215b504c

Child:

9:c49363372755

--- a/main.cpp	Wed Oct 30 12:25:09 2013 +0000
+++ b/main.cpp	Wed Oct 30 13:16:12 2013 +0000
@@ -111,14 +111,11 @@
     do {
         setpoint_beginA = -63;
         pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);   // + omdat men met een negatieve hoekverdraaiing werkt.
-        pc.printf("s: %f", pwm_to_begin_motorA);
-    } while(pwm_to_begin_motorA <= 0);
-    {
-        wait(0.5);
+        wait(0.2);
         keep_in_range(&pwm_to_begin_motorA, -1, 1 );
         motordirA.write(0);
         pwm_motorA.write(pwm_to_begin_motorA);
-    }
+    } while(pwm_to_begin_motorA <= 0);
     motorA.setPosition(0);
     pwm_motorA.write(0);
 
@@ -129,14 +126,11 @@
     do {
         setpoint_beginB = 192;
         pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
-        pc.printf("s: %f", pwm_to_begin_motorB);
-    } while(pwm_to_begin_motorB <= 0);
-    {
-        wait(0.5);
+        wait(0.2);
         keep_in_range(&pwm_to_begin_motorB, -1, 1 );
         motordirB.write(1);
         pwm_motorB.write(pwm_to_begin_motorB);
-    }
+    } while(pwm_to_begin_motorB <= 0);
     motorB.setPosition(0);
     pwm_motorB.write(0);
 
@@ -150,23 +144,64 @@
     pwm_motorB.write(0.08);
     pwm_motorA.write(0.08);
     do {
-        setpoint_beginA = -1000; //-532
+        setpoint_beginA = -532; 
         pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
-        setpoint_beginB = 1000; //460
+        setpoint_beginB = 460;
         pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
-    } while((pwm_to_begin_motorA <= 0)&&(pwm_to_begin_motorB <= 0)); {
-        wait(2);
+        wait(0.2);
         keep_in_range(&pwm_to_begin_motorB, -1, 1 );
         motordirB.write(1);
         pwm_motorB.write(pwm_to_begin_motorB);
         keep_in_range(&pwm_to_begin_motorA, -1, 1 );
         motordirA.write(0);
         pwm_motorA.write(pwm_to_begin_motorA);
-    }
+    } while((pwm_to_begin_motorA <= 0)&&(pwm_to_begin_motorB <= 0));
     pwm_motorA.write(0);
     pwm_motorB.write(0);
 
+    // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
+    // Hierna het script dat EMG wordt omgezet in een positie verandering
+    
+    /*Create a ticker, and let it call the     */
+    /*function 'setlooptimerflag' every 0.01s  */
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,0.01);
 
+//INFINITE LOOP
+    while(1) {
+        
+        while(looptimerflag != true);
+        looptimerflag = false;
+
+        // hier EMG
+        setpointA = (potmeterA.read()-0.09027)*(631); // bereik van 71 graden             dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen bij EMG wordt 0.5 - 0.09027
+        setpointB = (potmeterB.read())*(415);           // bereik van 46.7 graden
+        pc.printf("s: %f, %d ", setpointA, motorA.getPosition());       
+        
+        // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
+        // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
+        keep_in_range(&setpointA, -1105, -474);     // voor motor moet bereik zijn -1105 tot -474
+        keep_in_range(&setpointB, -147, 269);       // voor motor moet bereik zijn -147 tot 269
+
+        /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
+        pwm_to_motorA = (setpointA - motorA.getPosition())*.001;
+        pwm_to_motorB = (setpointB - motorB.getPosition())*.001;
+        
+        keep_in_range(&pwm_to_motorA, -1,1);
+        keep_in_range(&pwm_to_motorB, -1,1);
+
+        if(pwm_to_motorA > 0)
+            motordirA.write(1);
+        else
+            motordirA.write(0);
+        if(pwm_to_motorB > 0)
+            motordirB.write(1);
+        else
+            motordirB.write(0);
+
+        pwm_motorA.write(abs(pwm_to_motorA));
+        pwm_motorB.write(abs(pwm_to_motorB));
+    }
 }