Diff: main.cpp

Revision:

9:c4a2a99b61e0

Parent:

8:2a238dbd0386

Child:

10:761333231e50

--- a/main.cpp	Thu Mar 26 18:09:18 2015 +0000
+++ b/main.cpp	Thu Mar 26 18:48:36 2015 +0000
@@ -83,6 +83,7 @@
     bool turn_left=false;
     bool turn_right=false;
     bool need_decel=false;
+    int num_of_straight =0;
 
     //servo is offset zero by some bullshit number
     float bullshit_offset = .074;
@@ -133,7 +134,7 @@
             if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
                 current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
 
-            TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
+            //TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
         }// end motor enabled
         else {
             TFC_HBRIDGE_DISABLE;
@@ -232,7 +233,7 @@
                         turn_right=true;
                     }
                     
-                    // turn left hard
+                    // turn right hard
                     // wall is close to center on left
                       else if (num==2 and left_counter >-.2)
                     {
@@ -246,6 +247,8 @@
                    else if (turn_right == false and turn_left == false)
                     {
                         TFC_SetServo(0,(0.0+ bullshit_offset));
+                        TFC_SetMotorPWM(current_left_motor_speed-(.001*num_of_straight), current_right_motor_speed+(.001*num_of_straight)); // --left is faster, ++right is faster
+                        num_of_straight++;
                     }
                    
                     else{}
@@ -254,40 +257,58 @@
                     // set servo and motors according to how much left we need to turn
                     if(turn_left)
                     {
-                        turn_right = false;
-                        TFC_SetServo(0,left_counter + bullshit_offset );
+                        num_of_straight = 0; // no longer on a straight
+                        turn_right = false;  
+                        TFC_SetServo(0,left_counter + bullshit_offset ); // set turning servo
+                        
                         // normalize to center each frame
-                        left_counter += .01;        
+                        // left turning is - servo
+                        if(left_counter > -.2) // small turn, normalize quickly
+                            left_counter += .015;        
+                        else                // hard turn, normalize slowly
+                            left_counter += .004;
+                                
+                        // no longer turning boolean        
                         if (left_counter > (0+ bullshit_offset))
                             turn_left = false;
 
-                        if (need_decel)
+                        if (need_decel) // need to deal with the decel
                             {
                                 TFC_SetMotorPWM(current_left_motor_speed+(.55*left_counter), current_right_motor_speed-(.4*left_counter));  // ++left is slowed,--right is slowed 
                                 need_decel = false;
                             }
-                        else
+                        else  //  turning speeds 
                             TFC_SetMotorPWM(current_left_motor_speed+(.2*left_counter), current_right_motor_speed+(.35*left_counter));  //  ++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)
                     {
+                        num_of_straight = 0; // no longer going straight
                         turn_left =false;
-                        TFC_SetServo(0,right_counter - bullshit_offset);
+                        TFC_SetServo(0,right_counter - bullshit_offset); // set servo
+                        
                         // normalize to center each frame
-                        right_counter -= .01;        
+                        // right turning is + servo
+                        if(right_counter < .2) // small turn, normalize quickly
+                            right_counter -= .015;        
+                        else                // hard turn, normalize slowly
+                            right_counter -= .004;
+                        
+                        
+                        // no longer turning boolean
                         if (right_counter < (0+ bullshit_offset))
                             turn_right = false;
                             
-                        if(need_decel)
+                        if(need_decel)// need to deal with the decel
                             {
                                 TFC_SetMotorPWM(current_left_motor_speed+(.55*right_counter), current_right_motor_speed-(.4*right_counter));  // ++left is slowed,--right is slowed 
                                 need_decel = false;
                             }
-                        else    
+                        else    //  turning speeds 
                             TFC_SetMotorPWM(current_left_motor_speed-(.35*right_counter), current_right_motor_speed-(.2*right_counter));  //  --left is faster, --right is slowed      
-                        }   
+                        }   // end with turn right
+
 
                     // clearing values for next image processing round
                     black_value_count = 0;