Diff: main.cpp

Revision:

19:545e9ddba0e8

Parent:

17:b5886b554381

Child:

20:548b20634fb4

--- a/main.cpp	Thu Mar 26 21:37:13 2015 +0000
+++ b/main.cpp	Fri Mar 27 16:59:24 2015 +0000
@@ -6,7 +6,7 @@
 
 const float AGGRESSIVE = .55;
 const float MODERATE =.45;
-const float CONSERVATIVE =.39;
+const float CONSERVATIVE =.33;
 const float STOP =0;
 const float PROTECTION_THRESHOLD_UPPER =.7;
 const float PROTECTION_THRESHOLD_LOWER =-.7;
@@ -157,7 +157,7 @@
   
                     // checking for center line (single line)
                     for (uint16_t i=10; i<118; i++) {
-                        if ((*(TFC_LineScanImage0+i) < 450)) {
+                        if ((*(TFC_LineScanImage0+i) < 400)) {
                             black_values_list[black_value_count] = i;
                             black_value_count++;
                         }
@@ -197,6 +197,12 @@
                     if (black_value_count<2)
                         num = 0;
                     
+                    if (black_value_count>30)
+                    {
+                        while(1)
+                            TFC_SetMotorPWM(0, 0);
+                    }
+                    
                     TFC_SetBatteryLED(num);
                     
                     // best guess of center based on weighted average of history
@@ -210,10 +216,23 @@
                     {
                         //turn away a little bit for each frame that is wall
                         if (left_counter >-.45)
+                        {
                             //left_counter -=.05;    
                             //left_counter -=.03; 
                             //left_counter -= (.001428571*(128-center_now));
-                            left_counter -= (.002*(128-center_now));
+                            //left_counter -= (.002*(128-center_now));
+                            if (center_now>110)
+                                left_counter -=.03; 
+                            else
+                                left_counter -=.05;   
+                        }
+                                
+                        if (left_counter <-.46)
+                        {
+                            num_of_left =0;
+                            //need_decel=true;
+                        }
+                            
                             
                         turn_left=true;
                         turn_right=false;
@@ -226,6 +245,8 @@
                         left_counter=-0.54;
                         turn_left=true;
                         turn_right=false;
+                        
+                        num_of_left =0;
                         need_decel=true;
                     }
                     
@@ -233,9 +254,12 @@
                     // wall is close to center on left
                       else if (num==2 and left_counter >-.2)// only if we arent turning left
                     {
+                        
                         right_counter =.54;
                         turn_left=false;
                         turn_right=true;
+                        
+                        num_of_right =0;
                         need_decel=true;
                     }
                     
@@ -247,10 +271,22 @@
                     {    
                         //turn away a little bit for each frame that is wall
                         if (right_counter <.45)
+                        {
                             //right_counter +=.05;
                             //right_counter +=.03;
                             //right_counter +=(.001428571*center_now);
-                            right_counter +=(.002*center_now);
+                            //right_counter +=(.002*center_now);
+                            if (center_now<18)
+                                right_counter +=.03;
+                            else
+                                right_counter +=.05;
+                        }        
+              
+                        if (right_counter >.46)
+                        {
+                            num_of_right =0;
+                            //need_decel=true;
+                        }
               
                         turn_left=false;
                         turn_right=true;
@@ -284,9 +320,10 @@
                         // normalize to center each frame
                         // left turning is - servo
                         if(left_counter > -.2) // small turn, normalize quickly
-                            left_counter += .017;        
+                            //left_counter += .015;    
+                            left_counter += .010;      
                         else                // hard turn, normalize slowly
-                            left_counter += .01;
+                            left_counter += .009;
                                 
                         // no longer turning boolean        
                         if (left_counter > (0+ bullshit_offset))
@@ -294,7 +331,7 @@
 
                         if (need_decel) // need to deal with the decel
                             {
-                                TFC_SetMotorPWM(current_left_motor_speed+(.7*left_counter), current_right_motor_speed-(.5*left_counter));  // ++left is slowed,--right is slowed 
+                                TFC_SetMotorPWM(current_left_motor_speed+(.5*left_counter), current_right_motor_speed-(.4*left_counter));  // ++left is slowed,--right is slowed 
                                 need_decel = false;
                             }
                         else  //  turning speeds 
@@ -327,7 +364,7 @@
                             
                         if(need_decel)// need to deal with the decel
                             {
-                                TFC_SetMotorPWM(current_left_motor_speed+(.5*right_counter), current_right_motor_speed-(.7*right_counter));  // ++left is slowed,--right is slowed 
+                                TFC_SetMotorPWM(current_left_motor_speed+(.4*right_counter), current_right_motor_speed-(.5*right_counter));  // ++left is slowed,--right is slowed 
                                 need_decel = false;
                             }
                         else    //  turning speeds