Revision 9:c49363372755, committed 2013-10-31

Comitter:

Tess

Date:

Thu Oct 31 12:48:12 2013 +0000

Parent:

8:62e968f78878

Child:

10:9d7110f25908

Commit message:

Port PTC2 not working

Changed in this revision

--- a/main.cpp	Wed Oct 30 13:16:12 2013 +0000
+++ b/main.cpp	Thu Oct 31 12:48:12 2013 +0000
@@ -53,6 +53,7 @@
     float setpoint_beginB;
     float setpoint_rechtsonderA;
     float setpoint_rechtsonderB;
+
     /* variable to store pwm value in*/
     float pwm_to_motorA;
     float pwm_to_begin_motorA = 0;
@@ -60,6 +61,30 @@
     float pwm_to_motorB;
     float pwm_to_rechtsonder_motorA;
     float pwm_to_rechtsonder_motorB;
+
+    const float dt = 0.002;
+    float Kp = 0.001;  //0.0113
+    float Ki = 0.0759;
+    float Kd = 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;
@@ -104,12 +129,11 @@
     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.
-
+    
     motordirA.write(0);
     pwm_motorA.write(.08);
     do {
-        setpoint_beginA = -63;
+        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 );
@@ -121,10 +145,28 @@
 
     wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
 
+    // hierna moet motor A naar de rechtsonder A4. Motor A 532.
+   
+    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 B 21.6 (192) graden naar links om naar x-as te gaan.
+       
     motordirB.write(1);
     pwm_motorB.write(.08);
     do {
-        setpoint_beginB = 192;
+        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 );
@@ -135,33 +177,26 @@
     pwm_motorB.write(0);
 
     wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
-
-    // 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
-
+    
+    // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
+    
     motordirB.write(1);
-    motordirA.write(0);
     pwm_motorB.write(0.08);
-    pwm_motorA.write(0.08);
     do {
-        setpoint_beginA = -532; 
-        pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
-        setpoint_beginB = 460;
+        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);
-        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);
+    } while(pwm_to_begin_motorB <= 0);
     pwm_motorB.write(0);
-
+    
+    wait(1);
+    
     // 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;
@@ -169,24 +204,48 @@
 
 //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());       
-        
+        //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
 
-        /* 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;
-        
+        // 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;
+
+        // 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;
+
+        /* 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;
+
         keep_in_range(&pwm_to_motorA, -1,1);
         keep_in_range(&pwm_to_motorB, -1,1);