Diff: Control/drivecontrol.cpp

Revision:

28:600932142201

Parent:

27:b980fce784ea

Child:

30:daf286ac049f

--- a/Control/drivecontrol.cpp	Fri May 26 05:45:02 2017 +0000
+++ b/Control/drivecontrol.cpp	Sat May 27 07:21:02 2017 +0000
@@ -17,14 +17,17 @@
 // 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.3643f, RIGHT_WALL_THRES = 0.1932f;
-float RIGHT_SIDE_WALL_THRES = 0.32f, LEFT_SIDE_WALL_THRES = 0.28f;
+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;
 
 // Motor speed offsets
 float left_speed, right_speed, motor_speed;
 float MOTOR_BASE_SPEED = 12.0f;
 float cool_down_offset = 0.0f;
 
+bool enter_right_wall_pid_debug = false;
+
 // Encoder offsets
 int ENCODER_TURN_UNIT = 16000;
 
@@ -32,13 +35,14 @@
     // 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 = 5.0f;
+    float P = 18.0f, D = 2.0f;
     
     void navigate() {
         bool has_left_wall = leftDiagonalIR.readIR() > LEFT_WALL_THRES;
         bool has_right_wall = rightDiagonalIR.readIR() > RIGHT_WALL_THRES;
 
         if (has_left_wall && has_right_wall) {
+            enter_right_wall_pid_debug = false;
             if (rightDiagonalIR - RIGHT_WALL_THRES < 0) {
                 error_p = rightDiagonalIR - RIGHT_WALL_THRES;
             }
@@ -50,17 +54,24 @@
             }
             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 = 2 * (LEFT_WALL_THRES - leftDiagonalIR.readIR());
+           // error_p = 4 * (LEFT_WALL_THRES - 0.50 * leftDiagonalIR.readIR());
+           error_p = 4 * (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;
         }
         else if (has_right_wall) {
-            error_p = 2 * (rightDiagonalIR.readIR() - RIGHT_WALL_THRES);
+            error_p = 4 * (0.52 * 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;
         }
         else if (!has_left_wall && !has_right_wall) {
             error_p = 0;
             error_d = 0;
+            enter_right_wall_pid_debug = false;
         }
         total_error = P * error_p + D * (error_d - old_error_d);
         old_error_p = error_p;
@@ -121,6 +132,10 @@
         }
     }
 }
+
+bool DriveControl::right_wall_pid_debug() {
+    return enter_right_wall_pid_debug;
+}
 // Currently only have the x, y position fields for
 // each cell.
 DriveControl::DriveControl (int start_x, int start_y) {
@@ -143,7 +158,7 @@
     pc.printf("LEFT Encoder Reading %d\n\r", leftEncoder.getEncoderDistance(1));
     pc.printf("RIGHT Encoder Reading %d\n\r", rightEncoder.getEncoderDistance(0));
     pc.printf("FRONT TOP RIGHT IRSensor %f\n\r", rightFrontIR.readIR() * 1000);
-    pc.printf("FRONT TOP LEFT IRSensor %f\n\r", leftFrontIR.readIR() * 10);
+    pc.printf("FRONT TOP LEFT IRSensor %f\n\r", leftFrontIR.readIR() * 1000);
     pc.printf("LEFT Diagonal Sensor %f\n\r", leftDiagonalIR.readIR());
     pc.printf("RIGHT Diagonal Sensor %f\n\r", rightDiagonalIR.readIR());
     pc.printf("LEFT SIDE Sensor %f\n\r", leftIR.readIR());
@@ -208,8 +223,8 @@
 }
 
 bool DriveControl::has_front_wall() {
-    bool right_front_wall = (rightFrontIR.readIR() * 500) > FRONT_SENSOR_THRES;
-    bool left_front_wall = (leftFrontIR.readIR() * 10) > FRONT_SENSOR_THRES;
+    bool right_front_wall = (rightFrontIR.readIR() * 1000) > 272.5274f;
+    bool left_front_wall = (leftFrontIR.readIR() * 1000) > 344.3223f;
     return right_front_wall || left_front_wall;
 }