Revision 2:83dd9068b6c5, committed 2013-10-29

Comitter:

Tess

Date:

Tue Oct 29 16:02:31 2013 +0000

Parent:

1:7cafc9042056

Child:

3:c4df318913b8

Commit message:

jfidj
;

Changed in this revision

--- a/main.cpp	Fri Oct 25 09:22:16 2013 +0000
+++ b/main.cpp	Tue Oct 29 16:02:31 2013 +0000
@@ -49,17 +49,23 @@
     /* variable to store setpoint in */
     float setpointA;
     float setpointB;
+    float setpoint_beginA;
+    float setpoint_beginB;
+    float setpoint_rechtsonderA;
+    float setpoint_rechtsonderB;
     /* variable to store pwm value in*/
     float pwm_to_motorA;
+    float pwm_to_begin_motorA;
+    float pwm_to_begin_motorB;
     float pwm_to_motorB;
+    float pwm_to_rechtsonder_motorA;
+    float pwm_to_rechtsonder_motorB;
     int32_t positionmotorA_t0;
     int32_t positionmotorB_t0;
     int32_t positionmotorA_t_1;
     int32_t positionmotorB_t_1;
-    int32_t startingpositionmotorA;
-    int32_t startingpositionmotorB;
-    int32_t velocity_motorA;
-    int32_t velocity_motorB;
+    int32_t positiondifference_motorA;
+    int32_t positiondifference_motorB;
 
     //START OF CODE
 
@@ -67,84 +73,193 @@
     pc.baud(921600);
 
 
-    // in dit stukje code zorgen we ervoor dat de startpositie wordt bereikt.
-    motordirA.write(0);
-    pwm_motorA.write(.05);
-    positionmotorA_t0 = motorA.getPosition();
-    do {
-        wait(0.02);
-        positionmotorA_t_1 = positionmotorA_t0 ;
-        positionmotorA_t0 = motorA.getPosition();
-        velocity_motorA = (positionmotorA_t0 - positionmotorA_t_1) / 0.02;
-    } while(velocity_motorA <= 0.01);
-    startingpositionmotorA = motorA.getPosition();
-
+    // in dit stukje code zorgen we ervoor dat de startpositie wordt bereikt. Eerst B botsen dan A botsen
     motordirB.write(0);
-    pwm_motorB.write(.05);
+    pwm_motorB.write(.08);
     positionmotorB_t0 = motorB.getPosition();
     do {
-        wait(0.02);
+        wait(0.2);
         positionmotorB_t_1 = positionmotorB_t0;
         positionmotorB_t0 = motorB.getPosition();
-        velocity_motorB = (positionmotorB_t0 - positionmotorB_t_1) / 0.02;
-    } while(velocity_motorB <= 0.01);
-    startingpositionmotorB = motorB.getPosition();
+        positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
+    } while(positiondifference_motorB > 10);
+    motorB.setPosition(0);
+    pwm_motorB.write(0);
 
+    motordirA.write(1);
+    pwm_motorA.write(.08);
+    positionmotorA_t0 = motorA.getPosition();
+    do {
+        wait(0.2);
+        positionmotorA_t_1 = positionmotorA_t0 ;
+        positionmotorA_t0 = motorA.getPosition();
+        positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
+    } while(positiondifference_motorA > 10);
+    motorA.setPosition(0);
+    pwm_motorA.write(0);
+
+    // motor A moet als eerst naar x-as, moet 4.11 graden naar links om op de x-as te staan
+    // motor B moet 21.6 graden naar links.
+    // eerst motor A en dan motor B
 
-    /*Create a ticker, and let it call the     */
-    /*function 'setlooptimerflag' every 0.01s  */
-    Ticker looptimer;
-    looptimer.attach(setlooptimerflag,0.01);
+    // motordirA.write(1);
+    //setpoint_beginA = 63.5; // naar x-as gaan
+    //pwm_to_begin_motorA = setpoint_beginA *.001;
+    //keep_in_range(&pwm_to_begin_motorA, -1,1);
+    //pwm_motorA.write(abs(pwm_to_begin_motorA));
+    //motorA.setPosition(0);
+    //pwm_motorA.write(0);
 
-    //INFINITE LOOP
-    while(1) {
-        /* Wait until looptimer flag is true. */
-        /* '!=' means not-is-equal            */
+    //motordirB.write(1);
+    //setpoint_beginB = 192; // naar x-as gaan
+    //pwm_to_begin_motorB = setpoint_beginB *.001;
+    //keep_in_range(&pwm_to_begin_motorB, -1,1);
+    //pwm_motorB.write(abs(pwm_to_begin_motorB));
+    //motorB.setPosition(0);
+    //pwm_motorB.write(0);
+    Ticker looptimer1;
+    looptimer1.attach(setlooptimerflag,0.01);
+
+    while(pwm_to_begin_motorA >= 0) {
         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);
-        setpointB = (potmeterB.read()-0.5)*(871/2);  //1000 dan rotatie langzamer maken als lager maakt.
+        setpoint_beginA = 63;     // naar x-as gaan
+        pwm_to_begin_motorA = (setpoint_beginA - motorA.getPosition()) *.001;
+        keep_in_range(&pwm_to_begin_motorA, -1, 1 );
 
-        /* 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());
+        //if(pwm_to_begin_motorA <= 0) {
+        //  motordirA.write(0);
+        // motorA.setPosition(0);
+        //pwm_motorA.write(0);
+        //} else {
+        motordirA.write(0);
+        pwm_motorA.write(abs(pwm_to_begin_motorA));
+    }
+motorA.setPosition(0);
+pwm_motorA.write(0);
+
+Ticker looptimer2;
+looptimer2.attach(setlooptimerflag,0.01);
+
+while(pwm_to_begin_motorB >= 0)
+{
+    while(looptimerflag != true);
+    looptimerflag = false;
 
-        /* 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);
+    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);
+
+    looptimerflag = false;
 
-        /* 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);
+    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;
+
+    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));
+
+}
 
 
-        //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));
-    }
+/*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;
+
+
+
+
+    /* 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));
+}
 }