Revision 7:1f88215b504c, committed 2013-10-30

Comitter:

Tess

Date:

Wed Oct 30 12:25:09 2013 +0000

Parent:

6:2cfdda6721ab

Child:

8:62e968f78878

Commit message:

new with only the calibration stuff

Changed in this revision

--- a/main.cpp	Wed Oct 30 10:28:10 2013 +0000
+++ b/main.cpp	Wed Oct 30 12:25:09 2013 +0000
@@ -72,19 +72,23 @@
     /*Set the baudrate (use this number in RealTerm too!) */
     pc.baud(921600);
 
-    // in dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen
- /*   motordirB.write(0);
+    // in dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen.
+    // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
+
+    motordirB.write(0);
     pwm_motorB.write(.08);
     positionmotorB_t0 = motorB.getPosition();
     do {
         wait(0.2);
-        positionmotorB_t_1 = positionmotorB_t0;
+        positionmotorB_t_1 = positionmotorB_t0 ;
         positionmotorB_t0 = motorB.getPosition();
         positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
     } while(positiondifference_motorB > 10);
     motorB.setPosition(0);
     pwm_motorB.write(0);
 
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+
     motordirA.write(1);
     pwm_motorA.write(.08);
     positionmotorA_t0 = motorA.getPosition();
@@ -97,19 +101,20 @@
     motorA.setPosition(0);
     pwm_motorA.write(0);
 
-    // Hierna willen we de motor van zijn aller uiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links.
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+
+    // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links.
     // Hierna moet motor B 21.6 (192) graden naar links. Dus eerst motor A en dan motor B.
-*/
-    Ticker looptimer;
-    looptimer.attach(setlooptimerflag,0.01);
 
+    motordirA.write(0);
+    pwm_motorA.write(.08);
     do {
-       pwm_to_begin_motorA = abs((setpoint_beginA - motorA.getPosition()) *.001);
-    } while(pwm_to_begin_motorA <= 0.001); {
-        while(looptimerflag != true);
-        looptimerflag = false;
-        setpoint_beginA = -1000;     //-63 negatief omdat de motor op zijn kop hangt
-        //pwm_to_begin_motorA = abs((setpoint_beginA - motorA.getPosition()) *.001);
+        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);
         keep_in_range(&pwm_to_begin_motorA, -1, 1 );
         motordirA.write(0);
         pwm_motorA.write(pwm_to_begin_motorA);
@@ -117,144 +122,54 @@
     motorA.setPosition(0);
     pwm_motorA.write(0);
 
-    do{
-    pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
-    }
-    while(pwm_to_begin_motorB <= 0.001); {
-        while(looptimerflag != true);
-        looptimerflag = false;
-        setpoint_beginB = 1000;      //192
-        keep_in_range(&pwm_to_begin_motorB, -1,1);
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+
+    motordirB.write(1);
+    pwm_motorB.write(.08);
+    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);
+        keep_in_range(&pwm_to_begin_motorB, -1, 1 );
         motordirB.write(1);
         pwm_motorB.write(pwm_to_begin_motorB);
     }
     motorB.setPosition(0);
     pwm_motorB.write(0);
 
-// nu naar positie rechtsonderhoek A4 deze is voor motor A 532 en voor motor B 460
-
-//while(0) ///1
-//{
-//    while(looptimerflag != true);
-//
-//    looptimerflag = false;
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
 
-//   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));
+    // Hierna willen we de motor van zijn x-as naar de rechtsonder positie van A4 te krijgen. Volgorde van motoren maakt nu niet uit.
+    // nu naar positie rechtsonderhoek A4 deze is voor motor A 532 en voor motor B 460
 
-//    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
-    //      motordirB.write(1);
-//    pwm_motorB.write(abs(pwm_to_rechtsonder_motorB));
-
-//}
-
-    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;
+    motordirB.write(1);
+    motordirA.write(0);
+    pwm_motorB.write(0.08);
+    pwm_motorA.write(0.08);
+    do {
+        setpoint_beginA = -1000; //-532
+        pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
+        setpoint_beginB = 1000; //460
+        pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
+    } while((pwm_to_begin_motorA <= 0)&&(pwm_to_begin_motorB <= 0)); {
+        wait(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(abs(pwm_to_begin_motorA));
-
-        while(looptimerflag != true);
-        looptimerflag = false;
-
-        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_begin_motorB));
+        pwm_motorA.write(pwm_to_begin_motorA);
     }
     pwm_motorA.write(0);
     pwm_motorB.write(0);
 
 
-
-    /*Create a ticker, and let it call the     */
-    /*function 'setlooptimerflag' every 0.01s  */
-
-//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;
-
-
-
-
-        /* 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);
-
-        /* 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);
-
-        /* 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));
-    }
 }
 
 
-//coerces value 'in' to min or max when exceeding those values
-//if you'd like to understand the statement below take a google for
-//'ternary operators'.
 void keep_in_range(float * in, float min, float max)
 {
 *in > min ? *in < max? : *in = max: *in = min;
@@ -262,3 +177,7 @@
 
 
 
+
+
+
+