Diff: main.cpp

Revision:

13:fe2efec19eda

Parent:

12:ff7947a79149

--- a/main.cpp	Fri Oct 28 14:17:12 2016 +0000
+++ b/main.cpp	Tue Nov 01 09:18:58 2016 +0000
@@ -24,8 +24,9 @@
 volatile float q2 = 0;
 volatile float q1_begin;
 volatile float q2_begin;
+const float pi = 3.14159265359;
 const float l1 = 0.3626;
-const float l2 = 0.420;
+const float l2 = (0.420-0.065);     // middelpunt swiffer
 volatile float q1_v;
 volatile float q2_v;
 volatile float q1_ref = 0;
@@ -70,13 +71,6 @@
     //pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f \tTotErr1 = %0.9f \tTotErr2 = %0.9f\n\r",q1_error,q2_error,q1IntError,q2IntError,q1_total_error,q2_total_error);
 }
 
-volatile bool go_flag_initialize = false;
-
-void flag_initialize()
-{
-    go_flag_initialize = true;
-}
-
 Ticker PIDcontrol;
 volatile bool go_flag_controller = false;
 
@@ -89,34 +83,36 @@
 
 void begin_hoeken()
 {
-    wait(3);
+    wait(1.5);
     q1_ref = wheel_M1.getPulses()/(1334.355/2);
     q2_ref = wheel_M2.getPulses()/(1334.355/2);
     active_PID_ticker = true;
 }
 
-
 void initialize()
 {
     dir_M1 = 0; //ccw
     dir_M2 = 1; //cw
-    while (q1 < 40*2*3.1415/360) {
+    while ( (q1 < 20*2*pi/360) || (q2 > -45*2*pi/360) ) {
         q1 = wheel_M1.getPulses()/(1334.355/2);
-        pwm_M1 = 0.05;
+        q2 = wheel_M2.getPulses()/(1334.355/2);
+        if (q1 < 20*2*pi/360) {
+            pwm_M1 = 0.05;
+        } else {
+            pwm_M1 = 0;
+        }
+        if (q2 > -45*2*pi/360) {
+            pwm_M2 = 0.06;
+        } else {
+            pwm_M2 = 0;
+        }
         wait(0.005f);
     }
     pwm_M1 = 0;
-
-    while (q2 > -90*2*3.1415/360) {
-        q2 = wheel_M2.getPulses()/(1334.355/2);
-        pwm_M2 = 0.05;
-        wait(0.005f);
-    }
     pwm_M2 = 0;
     begin_hoeken();
 }
 
-
 void biceps()
 {
     q1_ref_prev = 0;
@@ -170,7 +166,7 @@
         q1_error_prev = 0;
         q2_error_prev = 0;
         q1_total_error = 0;
-        q1_total_error = 0;
+        q2_total_error = 0;
         pc.printf("\n\rSWITCH!!! \n\n\r");
     }
 
@@ -211,29 +207,29 @@
 
     //pc.printf("q1 = %f \tq2 = %f \tq1_v = %f \tq2_v = %f\n\r", q1,q2,q1_v,q2_v);
 
-    if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
+    if ( (q1 > (135*2*pi/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
         q1_v = 0;
         q2_v = 0;
-        q1_ref_prev = 0;
-        q2_ref_prev = 0;
+        q1_ref = q1;
+        q2_ref = q2;
         q1IntError = 0;
         q2IntError = 0;
         q1_error_prev = 0;
         q2_error_prev = 0;
-    } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
+    } else if ( (q1 < -(135*2*pi/360)) && (q1_v < 0) ) {
         q1_v = 0;
         q2_v = 0;
-        q1_ref_prev = 0;
-        q2_ref_prev = 0;
+        q1_ref = q1;
+        q2_ref = q2;
         q1IntError = 0;
         q2IntError = 0;
         q1_error_prev = 0;
         q2_error_prev = 0;
-    } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) {        // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
+    } else if ( (q2 < (-2.6)) && (q2_v < 0) ) {        // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
         q1_v = 0;
         q2_v = 0;
-        q1_ref_prev = 0;
-        q2_ref_prev = 0;
+        q1_ref = q1;
+        q2_ref = q2;
         q1IntError = 0;
         q2IntError = 0;
         q1_error_prev = 0;
@@ -241,8 +237,8 @@
     } else if ( (q2 >= 0) && (q2_v > 0) ) {
         q1_v = 0;
         q2_v = 0;
-        q1_ref_prev = 0;
-        q2_ref_prev = 0;
+        q1_ref = q1;
+        q2_ref = q2;
         q1IntError = 0;
         q2IntError = 0;
         q1_error_prev = 0;