Diff: main.cpp

Revision:

12:915f22e7d7d9

Parent:

11:e04dc2090433

Child:

13:8a667757beb3

--- a/main.cpp	Thu Mar 26 19:14:36 2015 +0000
+++ b/main.cpp	Thu Mar 26 19:41:04 2015 +0000
@@ -84,6 +84,8 @@
     bool turn_right=false;
     bool need_decel=false;
     int num_of_straight =0;
+    int num_of_left =0;
+    int num_of_right=0;
 
     //servo is offset zero by some bullshit number
     float bullshit_offset = .074;
@@ -201,12 +203,12 @@
                     
                     // turn left
                     // hit wall a little bit on the right
-                    if (num==8 and right_counter <.2 )
+                    if (num==8 and right_counter <.2 ) // only if we arent turning right
                     {
                         //turn away a little bit for each frame that is wall
                         if (left_counter >-.4)
-                            //left_counter -=.05;    
-                            left_counter -=.03; 
+                            left_counter -=.05;    
+                            //left_counter -=.03; 
                             
                         turn_left=true;
                         turn_right=false;
@@ -214,7 +216,7 @@
                     
                     // turn left real hard
                     // wall is close to center on right
-                    if (num==4 and right_counter <.2)
+                    if (num==4 and right_counter <.2) // only if we arent turning right
                     {
                         left_counter=-0.5;
                         turn_left=true;
@@ -224,12 +226,12 @@
                     
                     // turn right
                     // hit wall a little bit on the left
-                    else if (num==1 and left_counter >-.2)
+                    else if (num==1 and left_counter >-.2) // only if we arent turning left
                     {    
                         //turn away a little bit for each frame that is wall
                         if (right_counter <.4)
-                            //right_counter +=.05;
-                            right_counter +=.03;
+                            right_counter +=.05;
+                            //right_counter +=.03;
               
                         turn_left=false;
                         turn_right=true;
@@ -237,7 +239,7 @@
                     
                     // turn right hard
                     // wall is close to center on left
-                      else if (num==2 and left_counter >-.2)
+                      else if (num==2 and left_counter >-.2)// only if we arent turning left
                     {
                         right_counter =.5;
                         turn_left=false;
@@ -250,6 +252,8 @@
                     {
                         TFC_SetServo(0,(0.0+ bullshit_offset));
                         TFC_SetMotorPWM(current_left_motor_speed-(.00005*num_of_straight), current_right_motor_speed+(.00005*num_of_straight)); // --left is faster, ++right is faster
+                        num_of_left = 0;
+                        num_of_right= 0;
                         if (violence_level !=0)
                             num_of_straight++;
                     }
@@ -261,13 +265,15 @@
                     if(turn_left and (violence_level !=0))
                     {
                         num_of_straight = 0; // no longer on a straight
+                        num_of_right = 0;
+                        num_of_left++;
                         turn_right = false;  
                         TFC_SetServo(0,left_counter + bullshit_offset ); // set turning servo
                         
                         // normalize to center each frame
                         // left turning is - servo
                         if(left_counter > -.2) // small turn, normalize quickly
-                            left_counter += .015;        
+                            left_counter += .017;        
                         else                // hard turn, normalize slowly
                             left_counter += .01;
                                 
@@ -281,20 +287,23 @@
                                 need_decel = false;
                             }
                         else  //  turning speeds 
-                            TFC_SetMotorPWM(current_left_motor_speed+(.3*left_counter), current_right_motor_speed+(.3*left_counter));  //  ++left is slowed, ++right is faster
+                            TFC_SetMotorPWM(current_left_motor_speed+(.3*left_counter), current_right_motor_speed+(.3*left_counter)+(.00005*num_of_left));  //  ++left is slowed, ++right is faster
                         }// end of turn left
                     
                     // set servo and motors according to how much right we need to turn
                     if(turn_right and (violence_level !=0))
                     {
                         num_of_straight = 0; // no longer going straight
+                        num_of_right++;
+                        num_of_left=0;
+                        
                         turn_left =false;
                         TFC_SetServo(0,right_counter - bullshit_offset); // set servo
                         
                         // normalize to center each frame
                         // right turning is + servo
                         if(right_counter < .2) // small turn, normalize quickly
-                            right_counter -= .015;        
+                            right_counter -= .017;        
                         else                // hard turn, normalize slowly
                             right_counter -= .01;
                         
@@ -309,7 +318,7 @@
                                 need_decel = false;
                             }
                         else    //  turning speeds 
-                            TFC_SetMotorPWM(current_left_motor_speed-(.3*right_counter), current_right_motor_speed-(.3*right_counter));  //  --left is faster, --right is slowed      
+                            TFC_SetMotorPWM(current_left_motor_speed-(.3*right_counter)-(.00005*num_of_right), current_right_motor_speed-(.3*right_counter));  //  --left is faster, --right is slowed      
                         }   // end with turn right