Diff: Control/drivecontrol.cpp

Revision:

31:f7a8a9b82bc1

Parent:

30:daf286ac049f

--- a/Control/drivecontrol.cpp	Sun May 28 06:49:41 2017 +0000
+++ b/Control/drivecontrol.cpp	Sun May 28 09:54:40 2017 +0000
@@ -26,6 +26,12 @@
 float left_speed, right_speed, motor_speed;
 float MOTOR_BASE_SPEED = 20.0f; //12.0f;
 float cool_down_offset = 0.0f;
+//
+//    bool right_front_wall = (rightFrontIR.readIR() * 1000) > 355.0f;//350.39f;
+//    bool left_front_wall = (leftFrontIR.readIR() * 1000) > 315.0f;//310.25f;
+
+float RIGHT_FRONT_THRES = 355.0f;
+float LEFT_FRONT_THRES = 315.0f;
 
 bool enter_right_wall_pid_debug = false;
 
@@ -165,13 +171,19 @@
     pid_controller::turn(false);
 }
 
+void DriveControl::set_wall_follower_sensor_thres(){
+    //TODO
+    RIGHT_FRONT_THRES = 300.0f;
+    LEFT_FRONT_THRES = 250.0f;
+}
+
 void DriveControl::turn_right() {
     // TODO: Add PID Control
     pid_controller::turn(true);
 }
 
 void DriveControl::set_wall_follower_speed() {
-    MOTOR_BASE_SPEED  = 30;
+    MOTOR_BASE_SPEED  = 25;
 }
 
 void DriveControl::turn_around() {
@@ -222,8 +234,10 @@
 }
 
 bool DriveControl::has_front_wall() {
-    bool right_front_wall = (rightFrontIR.readIR() * 1000) > 355.0f;//350.39f;
-    bool left_front_wall = (leftFrontIR.readIR() * 1000) > 315.0f;//310.25f;
+//    float RIGHT_FRONT_THRES = 355.0f;
+//float LEFT_FRONT_THRES = 315.0f;
+    bool right_front_wall = (rightFrontIR.readIR() * 1000) > RIGHT_FRONT_THRES;//350.39f;
+    bool left_front_wall = (leftFrontIR.readIR() * 1000) > LEFT_FRONT_THRES;//310.25f;
     return right_front_wall || left_front_wall;
 }