Diff: main.cpp

Revision:

12:01250ea24795

Parent:

9:946d400b2f13

Child:

13:d49cb8b52a1e

--- a/main.cpp	Thu May 05 22:01:18 2016 +0000
+++ b/main.cpp	Thu May 05 23:56:48 2016 +0000
@@ -253,6 +253,7 @@
     Timer t;
     //initally put pen down
     servo_reset();
+    servo_stop();
     
     float currentAngle;
     float targetAngle;
@@ -288,6 +289,9 @@
     double sum_l = 0;
     double value_r;
     double value_l;
+    float angle_init;
+    
+    float testAngle = L.getAngle();
 
     while(1){
         // check what mode is present
@@ -395,7 +399,7 @@
         }
         else if (mode == 9){
             serial.printf("Draw coordinates \r\n");
-            draw_corr = 2; 
+            draw_corr = 1; 
             mode = -1;
         }
         
@@ -469,14 +473,14 @@
                     targetAngle = fmod(currentAngle+angle,360);
                     
                     servo_right();
-                    while(fmod(abs(L.getAngle() - targetAngle),360)> 3);
+                    while(fmod(abs(L.getAngle() - targetAngle),360)> 5);
                     servo_stop();
                 }
                 else if(dot < 0){
                     //serial.printf("Turn left \r\n");
                     targetAngle = fmod(currentAngle-angle,360);
                     servo_left();
-                    while(fmod(abs(L.getAngle() - targetAngle),360)> 3);
+                    while(fmod(abs(L.getAngle() - targetAngle),360)> 5);
                     servo_stop();
                 }
                 //compute length of path til target
@@ -508,7 +512,9 @@
                     on_l = 1;
                 }
                 serial.printf("Initially: left: %d, right: %d\r\n",on_l,on_r);
+                angle_init = L.getAngle();
                 servo_f();
+                serial.printf("Initial Angle: %f\r\n",angle_init);
                 //start decrement counter
                 counter_r = counter_m;
                 counter_l = counter_m;
@@ -523,24 +529,24 @@
                     value_l = sum_l/100;
                     //printf("Value: %f\r\n",value);
                     if(value_r < 0.93 && on_r == 1){
-                        printf("Value right: %f\r\n",value_r);
+                        //printf("Value right: %f\r\n",value_r);
                         on_r = 0;
                     }
                     else if (value_r > 0.95 && on_r == 0){
-                        printf("Value right: %f\r\n",value_r);
+                        //printf("Value right: %f\r\n",value_r);
                         on_r = 1;
                         counter_r--;
-                        printf("*** Right Counter is: %d\r\n",counter_r);
+                        //printf("*** Right Counter is: %d\r\n",counter_r);
                     }
                     if(value_l < 0.93 && on_l == 1){
-                        printf("Value left: %f\r\n",value_l);
+                        //printf("Value left: %f\r\n",value_l);
                         on_l = 0;
                     }
                     else if (value_l > 0.95 && on_l == 0){
-                        printf("Value left: %f\r\n",value_l);
+                        //printf("Value left: %f\r\n",value_l);
                         on_l = 1;
                         counter_l--;
-                        printf("*** Left Counter is: %d\r\n",counter_l);
+                        //printf("*** Left Counter is: %d\r\n",counter_l);
                     }
                     
                     if(counter_l >= counter_r){
@@ -549,7 +555,20 @@
                     else{
                         counter_m = counter_l;    
                     }
-                    printf("*** Distance to take is: %d\r\n",counter_m);
+                    //deviation left from path
+                    if(L.getAngle() - angle_init > 2 || (angle_init > 358 &&  L.getAngle() < 5 && (L.getAngle() + 360 - angle_init) > 2)){
+                        servo_left();
+                        serial.printf("Angle Difference: %f\r\n",L.getAngle() - angle_init);
+                        while(fmod(abs(L.getAngle() - angle_init),360)> 1);
+                        servo_f();                        
+                    }
+                    //deviation right from path
+                    if(angle_init - L.getAngle() > 2 ){
+                        servo_right();
+                        while(fmod(abs(L.getAngle() - angle_init),360)> 1);
+                        servo_f();                        
+                    }
+                    //printf("*** Distance to take is: %d\r\n",counter_m);
                 }
                 //servo_f();
                 //wait(distance*time_factor);