Revision 8:2a238dbd0386, committed 2015-03-26

Comitter:

mperella

Date:

Thu Mar 26 18:09:18 2015 +0000

Parent:

7:f21986164bf1

Child:

9:c4a2a99b61e0

Commit message:

fixed the bad decel code, more decel.

Changed in this revision

--- a/main.cpp	Thu Mar 26 15:51:49 2015 +0000
+++ b/main.cpp	Thu Mar 26 18:09:18 2015 +0000
@@ -5,7 +5,7 @@
 //#include "serialib.h"
 
 const float AGGRESSIVE = .55;
-const float MODERATE =.48;
+const float MODERATE =.45;
 const float CONSERVATIVE =.39;
 const float STOP =0;
 const float PROTECTION_THRESHOLD_UPPER =.7;
@@ -70,6 +70,10 @@
     int set_point = 63;
     int previous_error = 0;
     int error = 0;
+    
+    float increment = 0;
+    int right_turn_count = 0;
+    int left_turn_count = 0;
 
     for(int i = 0; i < 50; i++)
         centers_List[i] = 63;
@@ -97,14 +101,17 @@
             if (violence_level==3) {
                 current_left_motor_speed  = -(AGGRESSIVE);
                 current_right_motor_speed = AGGRESSIVE;
+                increment = 0.02;
             }
             else if (violence_level==2) {
                 current_left_motor_speed  = -(MODERATE);
                 current_right_motor_speed = (MODERATE);
+                increment = 0.03;
             }
             else if (violence_level==1) {
                 current_left_motor_speed  = -(CONSERVATIVE);
                 current_right_motor_speed = CONSERVATIVE;
+                increment = 0.04;
             }
             else if (violence_level==0) {
                 current_left_motor_speed  = STOP;
@@ -197,7 +204,7 @@
                     {
                         //turn away a little bit for each frame that is wall
                         if (left_counter >-.4)
-                            left_counter -=.04;    
+                            left_counter -=.05;    
                             
                         turn_left=true;
                         turn_right=false;
@@ -219,7 +226,7 @@
                     {    
                         //turn away a little bit for each frame that is wall
                         if (right_counter <.4)
-                            right_counter +=.04;
+                            right_counter +=.05;
               
                         turn_left=false;
                         turn_right=true;
@@ -248,7 +255,7 @@
                     if(turn_left)
                     {
                         turn_right = false;
-                        TFC_SetServo(0,left_counter);
+                        TFC_SetServo(0,left_counter + bullshit_offset );
                         // normalize to center each frame
                         left_counter += .01;        
                         if (left_counter > (0+ bullshit_offset))
@@ -256,18 +263,18 @@
 
                         if (need_decel)
                             {
-                                TFC_SetMotorPWM(current_left_motor_speed+(.3*left_counter), current_right_motor_speed-(.2*left_counter));  // ++left is slowed,--right is slowed 
+                                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
-                            TFC_SetMotorPWM(current_left_motor_speed+(.1*left_counter), current_right_motor_speed+(.3*left_counter));  //  ++left is slowed, ++right is faster
+                            TFC_SetMotorPWM(current_left_motor_speed+(.2*left_counter), current_right_motor_speed+(.35*left_counter));  //  ++left is slowed, ++right is faster
                         }
                     
                     // set servo and motors according to how much right we need to turn
                     if(turn_right)
                     {
                         turn_left =false;
-                        TFC_SetServo(0,right_counter);
+                        TFC_SetServo(0,right_counter - bullshit_offset);
                         // normalize to center each frame
                         right_counter -= .01;        
                         if (right_counter < (0+ bullshit_offset))
@@ -275,11 +282,11 @@
                             
                         if(need_decel)
                             {
-                                TFC_SetMotorPWM(current_left_motor_speed+(.2*left_counter), current_right_motor_speed-(.3*left_counter));  // ++left is slowed,--right is slowed 
+                                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    
-                            TFC_SetMotorPWM(current_left_motor_speed-(.3*right_counter), current_right_motor_speed-(.1*right_counter));  //  --left is faster, --right is slowed      
+                            TFC_SetMotorPWM(current_left_motor_speed-(.35*right_counter), current_right_motor_speed-(.2*right_counter));  //  --left is faster, --right is slowed      
                         }   
 
                     // clearing values for next image processing round