Revision 3:c4df318913b8, committed 2013-10-30

Comitter:

Tess

Date:

Wed Oct 30 08:41:50 2013 +0000

Parent:

2:83dd9068b6c5

Child:

4:8344a3edd96c

Commit message:

gd
;

Changed in this revision

--- a/main.cpp	Tue Oct 29 16:02:31 2013 +0000
+++ b/main.cpp	Wed Oct 30 08:41:50 2013 +0000
@@ -137,129 +137,156 @@
         motordirA.write(0);
         pwm_motorA.write(abs(pwm_to_begin_motorA));
     }
-motorA.setPosition(0);
-pwm_motorA.write(0);
+    motorA.setPosition(0);
+    pwm_motorA.write(0);
 
-Ticker looptimer2;
-looptimer2.attach(setlooptimerflag,0.01);
+    Ticker looptimer2;
+    looptimer2.attach(setlooptimerflag,0.01);
 
-while(pwm_to_begin_motorB >= 0)
-{
-    while(looptimerflag != true);
-    looptimerflag = false;
+    while(pwm_to_begin_motorB >= 0) {
+        while(looptimerflag != true);
+        looptimerflag = false;
 
-    setpoint_beginB = 192; //192
-    pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001;
-    keep_in_range(&pwm_to_begin_motorB, -1,1);
-    //if(pwm_to_begin_motorB <= 0) {
-    //  motordirB.write(0);
-    //motorB.setPosition(0);
-    //pwm_motorB.write(0);
-    //} else {
-    motordirB.write(1);
-    pwm_motorB.write(abs(pwm_to_begin_motorB));
-}
-motorB.setPosition(0);
-pwm_motorB.write(0);
+        setpoint_beginB = 192; //192
+        pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001;
+        keep_in_range(&pwm_to_begin_motorB, -1,1);
+        //if(pwm_to_begin_motorB <= 0) {
+        //  motordirB.write(0);
+        //motorB.setPosition(0);
+        //pwm_motorB.write(0);
+        //} else {
+        motordirB.write(1);
+        pwm_motorB.write(abs(pwm_to_begin_motorB));
+    }
+    motorB.setPosition(0);
+    pwm_motorB.write(0);
 
 // nu naar positie rechtsonderhoek A4 deze is motor A 531.6 en motor B 460
-// verder werken vanaf hier
-while(0) ///1
-{
-    while(looptimerflag != true);
+
+//while(0) ///1
+//{
+//    while(looptimerflag != true);
+//
+//    looptimerflag = false;
+
+//   setpoint_rechtsonderA = 531;     // naar rechter onderhoek
+    // pwm_to_rechtsonder_motorA = setpoint_rechtsonderA *.001;
+    //  keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
+//    if(pwm_to_rechtsonder_motorA > 0.531) {
+//        motordirA.write(0);
+//        pwm_motorA.write(0);
+//    } else
+//        motordirA.write(1);
+//    pwm_motorA.write(abs(pwm_to_rechtsonder_motorA));
+
+//    while(looptimerflag != true);
+//    looptimerflag = false;
 
-    looptimerflag = false;
+//    setpoint_rechtsonderB = 460;
+//    pwm_to_rechtsonder_motorB = setpoint_rechtsonderB *.001;
+//    keep_in_range(&pwm_to_rechtsonder_motorB, -1,1);
+//    if(pwm_to_rechtsonder_motorB > 0.460) {
+//        motordirB.write(0);
+//        pwm_motorB.write(0);
+//    } else
+    //      motordirB.write(1);
+//    pwm_motorB.write(abs(pwm_to_rechtsonder_motorB));
+
+//}
 
-    setpoint_rechtsonderA = 531;     // naar rechter onderhoek
-    pwm_to_rechtsonder_motorA = setpoint_rechtsonderA *.001;
-    keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
-    if(pwm_to_rechtsonder_motorA > 0.531) {
+    Ticker looptimer3;
+    looptimer3.attach(setlooptimerflag,0.01);
+
+    while((pwm_to_begin_motorA >= 0)&&(pwm_to_begin_motorB >= 0)) {
+        while(looptimerflag != true);
+
+        looptimerflag = false;
+
+        setpoint_beginA = 1000;//532     // naar rechteronderhoek A4
+        pwm_to_begin_motorA = (setpoint_beginA - motorA.getPosition()) *.001;
+        keep_in_range(&pwm_to_begin_motorA, -1, 1 );
+
         motordirA.write(0);
-        pwm_motorA.write(0);
-    } else
-        motordirA.write(1);
-    pwm_motorA.write(abs(pwm_to_rechtsonder_motorA));
+        pwm_motorA.write(abs(pwm_to_begin_motorA));
 
-    while(looptimerflag != true);
-    looptimerflag = false;
+        while(looptimerflag != true);
+
+        looptimerflag = false;
 
-    setpoint_rechtsonderB = 460;
-    pwm_to_rechtsonder_motorB = setpoint_rechtsonderB *.001;
-    keep_in_range(&pwm_to_rechtsonder_motorB, -1,1);
-    if(pwm_to_rechtsonder_motorB > 0.460) {
-        motordirB.write(0);
-        pwm_motorB.write(0);
-    } else
+        setpoint_beginB = 1000; //460
+        pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001;
+        keep_in_range(&pwm_to_begin_motorB, -1,1);
         motordirB.write(1);
-    pwm_motorB.write(abs(pwm_to_rechtsonder_motorB));
-
-}
+        pwm_motorB.write(abs(pwm_to_begin_motorB));
+    }
+    pwm_motorA.write(0);
+    pwm_motorB.write(0);
 
 
-/*Create a ticker, and let it call the     */
-/*function 'setlooptimerflag' every 0.01s  */
-Ticker looptimer;
-looptimer.attach(setlooptimerflag,0.01);
+
+    /*Create a ticker, and let it call the     */
+    /*function 'setlooptimerflag' every 0.01s  */
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,0.01);
 
 //INFINITE LOOP
-while(1)
-{
-    /* Wait until looptimer flag is true. */
-    /* '!=' means not-is-equal            */
-    while(looptimerflag != true);
-    /* Clear the looptimerflag, otherwise */
-    /* the program would simply continue  */
-    /* without waitingin the next iteration*/
-    looptimerflag = false;
+    while(1) {
+        /* Wait until looptimer flag is true. */
+        /* '!=' means not-is-equal            */
+        while(looptimerflag != true);
+        /* Clear the looptimerflag, otherwise */
+        /* the program would simply continue  */
+        /* without waitingin the next iteration*/
+        looptimerflag = false;
 
 
 
 
-    /* Read potmeter value, apply some math */
-    /* to get useful setpoint value         */
-    setpointA = (potmeterA.read()-0.5)*(631/2); // bereik van 71 graden             dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen
-    setpointB = (potmeterB.read()-0.5)*(415/2);  // bereik van 46.7 graden           1000 dan rotatie langzamer maken als lager maakt.
+        /* Read potmeter value, apply some math */
+        /* to get useful setpoint value         */
+        setpointA = (potmeterA.read()-0.5)*(631/2); // bereik van 71 graden             dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen
+        setpointB = (potmeterB.read()-0.5)*(415/2);  // bereik van 46.7 graden           1000 dan rotatie langzamer maken als lager maakt.
 
 
-    // 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, 474, 1105);
-    keep_in_range(&setpointB, -147, 269);
+        // 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, 474, 1105);
+        keep_in_range(&setpointB, -147, 269);
 
-    /* Print setpoint and current position to serial terminal*/
-    pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
-    pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition());
+        /* Print setpoint and current position to serial terminal*/
+        pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
+        pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition());
 
-    /* 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;
-    /* Coerce pwm value if outside range          */
-    /* Not strictly needed here, but useful       */
-    /* if doing other calculations with pwm value */
-    keep_in_range(&pwm_to_motorA, -1,1);
-    keep_in_range(&pwm_to_motorB, -1,1);
+        /* 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;
+        /* Coerce pwm value if outside range          */
+        /* Not strictly needed here, but useful       */
+        /* if doing other calculations with pwm value */
+        keep_in_range(&pwm_to_motorA, -1,1);
+        keep_in_range(&pwm_to_motorB, -1,1);
 
-    /* Control the motor direction pin. based on   */
-    /* the sign of your pwm value.      If your    */
-    /* motor keeps spinning when running this code */
-    /* you probably need to swap the motor wires,  */
-    /* or swap the 'write(1)' and 'write(0)' below */
-    if(pwm_to_motorA > 0)
-        motordirA.write(1);
-    else
-        motordirA.write(0);
-    if(pwm_to_motorB > 0)
-        motordirB.write(1);
-    else
-        motordirB.write(0);
+        /* Control the motor direction pin. based on   */
+        /* the sign of your pwm value.      If your    */
+        /* motor keeps spinning when running this code */
+        /* you probably need to swap the motor wires,  */
+        /* or swap the 'write(1)' and 'write(0)' below */
+        if(pwm_to_motorA > 0)
+            motordirA.write(1);
+        else
+            motordirA.write(0);
+        if(pwm_to_motorB > 0)
+            motordirB.write(1);
+        else
+            motordirB.write(0);
 
 
-    //WRITE VALUE TO MOTOR
-    /* Take the absolute value of the PWM to send */
-    /* to the motor. */
-    pwm_motorA.write(abs(pwm_to_motorA));
-    pwm_motorB.write(abs(pwm_to_motorB));
-}
+        //WRITE VALUE TO MOTOR
+        /* Take the absolute value of the PWM to send */
+        /* to the motor. */
+        pwm_motorA.write(abs(pwm_to_motorA));
+        pwm_motorB.write(abs(pwm_to_motorB));
+    }
 }