Diff: Control/drivecontrol.cpp

Revision:

30:daf286ac049f

Parent:

28:600932142201

Child:

31:f7a8a9b82bc1

--- a/Control/drivecontrol.cpp	Sat May 27 08:07:13 2017 +0000
+++ b/Control/drivecontrol.cpp	Sun May 28 06:49:41 2017 +0000
@@ -17,25 +17,26 @@
 // Sensor offsets
 //float FRONT_SENSOR_THRES = 7.0f, SENSOR_ERROR_OFFSET = 0.0f;
 float FRONT_SENSOR_THRES = 2.2f, SENSOR_ERROR_OFFSET = 0.0f;
-float LEFT_WALL_THRES = 0.375f, RIGHT_WALL_THRES = 0.1975f;
-//float RIGHT_SIDE_WALL_THRES = 0.59f, LEFT_SIDE_WALL_THRES = 0.38f;
-float RIGHT_SIDE_WALL_THRES = 0.28f, LEFT_SIDE_WALL_THRES = 0.38f;
+//float LEFT_WALL_THRES = 0.336f, RIGHT_WALL_THRES = 0.163f;
+float LEFT_WALL_THRES = 0.336f, RIGHT_WALL_THRES = 0.163f;
+float RIGHT_SIDE_WALL_THRES = 0.20f, LEFT_SIDE_WALL_THRES = 0.20f;
+//float RIGHT_SIDE_WALL_THRES = 0.28f, LEFT_SIDE_WALL_THRES = 0.38f;
 
 // Motor speed offsets
 float left_speed, right_speed, motor_speed;
-float MOTOR_BASE_SPEED = 12.0f;
+float MOTOR_BASE_SPEED = 20.0f; //12.0f;
 float cool_down_offset = 0.0f;
 
 bool enter_right_wall_pid_debug = false;
 
 // Encoder offsets
-int ENCODER_TURN_UNIT = 16000;
+int ENCODER_TURN_UNIT = 16500;
 
 namespace pid_controller { 
     // PID Constants
     float error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f;
     float total_error = 0.0f;
-    float P = 18.0f, D = 2.0f;
+    float P = 18.0f, D = 20.0f;
     
     void navigate() {
         bool has_left_wall = leftDiagonalIR.readIR() > LEFT_WALL_THRES;
@@ -47,24 +48,26 @@
                 error_p = rightDiagonalIR - RIGHT_WALL_THRES;
             }
             else if (leftDiagonalIR - LEFT_WALL_THRES < 0) {
-                error_p = leftDiagonalIR - LEFT_WALL_THRES;
+                error_p = LEFT_WALL_THRES - leftDiagonalIR;
             }
             else{
                 error_p = rightDiagonalIR - leftDiagonalIR;
+                enter_right_wall_pid_debug = true; 
             }
-            error_d = error_p - old_error_p;    
+            error_d = error_p - old_error_p;  
+             
         }
 //        float RIGHT_SIDE_WALL_THRES = 0.59f, LEFT_SIDE_WALL_THRES = 0.38f;
         else if (has_left_wall) {
            // error_p = 4 * (LEFT_WALL_THRES - 0.50 * leftDiagonalIR.readIR());
-           error_p = 4 * (LEFT_WALL_THRES - leftDiagonalIR.readIR());
+           error_p = LEFT_WALL_THRES - leftDiagonalIR.readIR();
             //error_p = 2 * (LEFT_SIDE_WALL_THRES - leftIR.readIR());
             error_d = error_p - old_error_p;
-            enter_right_wall_pid_debug = true;
+            enter_right_wall_pid_debug = false;
         }
         else if (has_right_wall) {
-            error_p = 4 * (0.52 * rightDiagonalIR.readIR() - RIGHT_WALL_THRES);
-            //error_p = 2 * (RIGHT_SIDE_WALL_THRES - rightIR.readIR());
+            error_p = rightDiagonalIR.readIR() - RIGHT_WALL_THRES;
+            //error_p = 2 * (RIGHT_SIDE_WALL_THRES - rightIR.readIR();
             error_d = error_p - old_error_p;
             enter_right_wall_pid_debug = false;
         }
@@ -101,14 +104,6 @@
         }
     }
     
-    void one_cell_turned(){
-        leftEncoder.reset();
-        rightEncoder.reset();
-        while(leftEncoder.getEncoderDistance(1)<-45000 & leftEncoder.getEncoderDistance(1)<45000){
-            //Do nothing
-        }
-    }
-    
     // should use this method as the exit condition
     // in the pid_controller::navigate() method
     // resets the pid, encoders, etc.
@@ -175,6 +170,10 @@
     pid_controller::turn(true);
 }
 
+void DriveControl::set_wall_follower_speed() {
+    MOTOR_BASE_SPEED  = 30;
+}
+
 void DriveControl::turn_around() {
     // TODO: Add PID Control
     //pid_controller::turn(TURN_AROUND);
@@ -219,12 +218,12 @@
     int max_two = 
         (- leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0))
         ? rightEncoder.getEncoderDistance(0):- leftEncoder.getEncoderDistance(1);
-    return max_two > 45000;
+    return max_two > 36000;
 }
 
 bool DriveControl::has_front_wall() {
-    bool right_front_wall = (rightFrontIR.readIR() * 1000) > 272.5274f;
-    bool left_front_wall = (leftFrontIR.readIR() * 1000) > 344.3223f;
+    bool right_front_wall = (rightFrontIR.readIR() * 1000) > 355.0f;//350.39f;
+    bool left_front_wall = (leftFrontIR.readIR() * 1000) > 315.0f;//310.25f;
     return right_front_wall || left_front_wall;
 }