Revision 11:2b4f12230ff0, committed 2013-11-01

Comitter:

Tess

Date:

Fri Nov 01 09:06:42 2013 +0000

Parent:

10:9d7110f25908

Child:

12:32dad0e2f141

Commit message:

Version 4: working with PD controller, no I controller needed

Changed in this revision

--- a/main.cpp	Thu Oct 31 13:45:13 2013 +0000
+++ b/main.cpp	Fri Nov 01 09:06:42 2013 +0000
@@ -8,18 +8,6 @@
 *                                                                              *
 ********************************************************************************/
 
-// dit is voor schakelaar die aan en uit wil gaan
-DigitalIn toggle(PTD7);
-
-void toggle_on()
-{
-}
-
-void toggle_off()
-{
-    // do nothing
-}
-
 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
 void keep_in_range(float * in, float min, float max);
 
@@ -76,27 +64,18 @@
 
     const float dt = 0.002;
     float Kp = 0.001;  //0.0113
-    float Ki = 0.0759;
-    float Kd = 0.0004342;
+    float Kd = 0.00004342;  //0.0004342
     float error_t0_A = 0;
     float error_t0_B = 0;
     float error_ti_A;
     float error_ti_B;
-    float error_t_1_A;
-    float error_t_1_B;
     float P_regelaar_A;
     float P_regelaar_B;
-    float I_regelaar_A;
-    float I_regelaar_B;
     float D_regelaar_A;
     float D_regelaar_B;
     float output_regelaar_A;
     float output_regelaar_B;
-    float integral_i_A;
-    float integral_i_B;
-    float integral_0_A = 0;
-    float integral_0_B = 0;
-
+    
     int32_t positionmotorA_t0;
     int32_t positionmotorB_t0;
     int32_t positionmotorA_t_1;
@@ -106,185 +85,168 @@
 
     //START OF CODE
 
-    while(1) {
-        while(!toggle);
-        { // wait while toggle == 0
-            toggle_on();
+    /*Set the baudrate (use this number in RealTerm too!) */
+    pc.baud(921600);
 
-            /*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.
-            // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).
+    // 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_t0 = motorB.getPosition();
-                positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
-            } while(positiondifference_motorB > 10);
-            motorB.setPosition(0);
-            pwm_motorB.write(0);
+    motordirB.write(0);
+    pwm_motorB.write(.08);
+    positionmotorB_t0 = motorB.getPosition();
+    do {
+        wait(0.2);
+        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.
+    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();
-            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);
+    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);
 
-            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.
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
 
-            motordirA.write(0);
-            pwm_motorA.write(.08);
-            do {
-                setpoint_beginA = -63;      // x-as
-                pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);   // + omdat men met een negatieve hoekverdraaiing werkt.
-                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);
+    // 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.
 
-            wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+    motordirA.write(0);
+    pwm_motorA.write(.08);
+    do {
+        setpoint_beginA = -63;      // x-as
+        pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);   // + omdat men met een negatieve hoekverdraaiing werkt.
+        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);
 
-            // hierna moet motor A naar de rechtsonder A4. Motor A 532.
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
 
-            motordirA.write(0);
-            pwm_motorA.write(0.08);
-            do {
-                setpoint_beginA = -532;     // rechtsonder positie A4
-                pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
-                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);
-            pwm_motorA.write(0);
-
-            wait(1);
+    // hierna moet motor A naar de rechtsonder A4. Motor A 532.
 
-            // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
+    motordirA.write(0);
+    pwm_motorA.write(0.08);
+    do {
+        setpoint_rechtsonderA = -532;     // rechtsonder positie A4
+        pwm_to_rechtsonder_motorA = abs((setpoint_rechtsonderA + motorA.getPosition()) *.001);
+        wait(0.2);
+        keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
+        motordirA.write(0);
+        pwm_motorA.write(pwm_to_rechtsonder_motorA);
+    } while(pwm_to_rechtsonder_motorA <= 0);
+    pwm_motorA.write(0);
 
-            motordirB.write(1);
-            pwm_motorB.write(.08);
-            do {
-                setpoint_beginB = 192;      // x-as
-                pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
-                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);
+    wait(1);
 
-            wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+    // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
 
-            // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
+    motordirB.write(1);
+    pwm_motorB.write(.08);
+    do {
+        setpoint_beginB = 192;      // x-as
+        pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
+        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);
 
-            motordirB.write(1);
-            pwm_motorB.write(0.08);
-            do {
-                setpoint_beginB = 460;      // rechtsonder positie A4
-                pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
-                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);
-            pwm_motorB.write(0);
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
 
-            wait(1);
-
-            // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
-            // Hierna het script dat EMG wordt omgezet in een positie verandering
+    // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
 
-            /*Create a ticker, and let it call the     */
-            /*function 'setlooptimerflag' every 0.01s  */
-            Ticker looptimer;
-            looptimer.attach(setlooptimerflag,0.01);
+    motordirB.write(1);
+    pwm_motorB.write(0.08);
+    do {
+        setpoint_rechtsonderB = 460;      // rechtsonder positie A4
+        pwm_to_rechtsonder_motorB = abs((setpoint_rechtsonderB - motorB.getPosition()) *.001);
+        wait(0.2);
+        keep_in_range(&pwm_to_rechtsonder_motorB, -1, 1 );
+        motordirB.write(1);
+        pwm_motorB.write(pwm_to_rechtsonder_motorB);
+    } while(pwm_to_rechtsonder_motorB <= 0);
+    pwm_motorB.write(0);
 
-            //INFINITE LOOP
-            while(1) {
-
-                while(looptimerflag != true);
-                looptimerflag = false;
+    wait(1);
 
-                // 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());
-                //pc.printf("s: %f, %d ", setpointB, motorB.getPosition());
-                setpointA = (potmeterA.read() - 0.5)*(631/2);
-                setpointB = (potmeterB.read() - 0.5) * (871/2);
-                // 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
+    // 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;
 
-                // PID regelaar voor motor A
-                //wait(dt);
-                //error_ti_A = setpointA - motorA.getPosition();
-                //P_regelaar_A = Kp * error_ti_A;
-                //D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
-                //integral_i_A = integral_0_A + (error_ti_A * dt);
-                //I_regelaar_A = Ki * integral_i_A;
-                //integral_0_A = integral_i_A;
-                //error_t0_A = error_ti_A;
-                //output_regelaar_A = P_regelaar_A;
+        // 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());
+        //pc.printf("s: %f, %d ", setpointB, motorB.getPosition());
+        setpointA = (potmeterA.read() - 0.5)*(631/2);
+        setpointB = (potmeterB.read() - 0.5) * (871/2);
 
-                // PID regelaar voor motor B
-                //wait(dt);
-                //error_ti_B = setpointB - motorB.getPosition();
-                //P_regelaar_B = Kp * error_ti_B;
-                //D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
-                //integral_i_B = integral_0_B + (error_ti_B * dt);
-                //I_regelaar_B = Ki * integral_i_B;
-                //integral_0_B = integral_i_B;
-                //error_t0_B = error_ti_B;
-                //output_regelaar_B = P_regelaar_B;
+        // 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
+        */
+
+        //PD regelaar voor motor A
+        wait(dt);
+        error_ti_A = setpointA - motorA.getPosition();
+        P_regelaar_A = Kp * error_ti_A;
+        D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
+        error_t0_A = error_ti_A;
+        output_regelaar_A = P_regelaar_A + D_regelaar_A;
 
-                /* This is a PID-action! store in pwm_to_motor */
-                pwm_to_motorA = (setpointA - motorA.getPosition())*.001;        //output_regelaar_A;
-                pwm_to_motorB = (setpointB - motorB.getPosition())*.001;        //output_regelaar_B;
+        //PD regelaar voor motor B
+        wait(dt);
+        error_ti_B = setpointB - motorB.getPosition();
+        P_regelaar_B = Kp * error_ti_B;
+        D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
+        error_t0_B = error_ti_B;
+        output_regelaar_B = P_regelaar_B + D_regelaar_B;
 
-                keep_in_range(&pwm_to_motorA, -1,1);
-                keep_in_range(&pwm_to_motorB, -1,1);
+        /* This is a PID-action! store in pwm_to_motor */
+        pwm_to_motorA = output_regelaar_A;
+        pwm_to_motorB = output_regelaar_B;
 
-                if(pwm_to_motorA > 0)
-                    motordirA.write(1);
-                else
-                    motordirA.write(0);
-                if(pwm_to_motorB > 0)
-                    motordirB.write(1);
-                else
-                    motordirB.write(0);
+        keep_in_range(&pwm_to_motorA, -1,1);
+        keep_in_range(&pwm_to_motorB, -1,1);
 
-                pwm_motorA.write(abs(pwm_to_motorA));
-                pwm_motorB.write(abs(pwm_to_motorB));
-            }
-        }
-        while(toggle);
-        {  // wait while toggle == 1
-            toggle_off();
-            pwm_motorA.write(0);
-            pwm_motorB.write(0);
-        }
+        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));
     }
 }