Diff: Control/drivecontrol.cpp

Revision:

16:c26d8e007df5

Parent:

15:151e59899221

Child:

17:043ed1d0196f

--- a/Control/drivecontrol.cpp	Sat May 20 05:17:47 2017 +0000
+++ b/Control/drivecontrol.cpp	Sat May 20 21:15:30 2017 +0000
@@ -16,14 +16,17 @@
 
 // Sensor offsets
 float FRONT_SENSOR_THRES = 7.0f, SENSOR_ERROR_OFFSET = 0.0f;
-float LEFT_WALL_THRES = 0.552f, RIGHT_WALL_THRES = 0.198f;
-float RIGHT_SIDE_WALL_THRES = 0.3f, LEFT_SIDE_WALL_THRES = 0.3f;
+float LEFT_WALL_THRES = 0.626f, RIGHT_WALL_THRES = 0.161f;
+float RIGHT_SIDE_WALL_THRES = 0.5f, LEFT_SIDE_WALL_THRES = 0.5f;
 
 // Motor speed offsets
 float left_speed, right_speed, motor_speed;
 float MOTOR_BASE_SPEED = 10.0f;
 float cool_down_offset = 0.0f;
 
+// Encoder offsets
+int ENCODER_TURN_UNIT = 16000;
+
 namespace pid_controller { 
     // PID Constants
     float error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f;
@@ -63,7 +66,7 @@
         old_error_d = error_d;
         
         //MOTOR_BASE_SPEED -= cool_down_offset;
-        if(total_error < 5.0f){
+        if(total_error < 7.5f){
             leftMotor -> speed(MOTOR_BASE_SPEED - total_error);
             rightMotor -> speed(MOTOR_BASE_SPEED + total_error);
             
@@ -80,6 +83,11 @@
         
     }
     
+    void clear_pid(){
+        error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f;
+        total_error = 0.0f;
+    }
+    
     void cool_down_speed() {
         if (cool_down_offset < 5.0f) {
             cool_down_offset += 2.0f;
@@ -105,7 +113,7 @@
     
     // TODO
     void turn (bool turn_right) {
-        float MOTOR_TURN_SPEED = 10.0f;
+        float MOTOR_TURN_SPEED = 14.0f;
         
         if (turn_right) {
             leftMotor -> speed(MOTOR_TURN_SPEED);
@@ -131,11 +139,6 @@
     return curr_cell;
 }
 
-void DriveControl::turn_left() {
-    // TODO: Add PID Control
-    pid_controller::turn(false);
-}
-
 int DriveControl::get_next_direction() {
     // TODO: Define the direction based on heuristic eval. 
     return 1;   
@@ -149,14 +152,17 @@
 void DriveControl::print_serial_ports(){
     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("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());
     pc.printf("RIGHT SIDE Sensor %f\n\r", rightIR.readIR());
-    */
+}
+
+void DriveControl::turn_left() {
+    // TODO: Add PID Control
+    pid_controller::turn(false);
 }
 
 void DriveControl::turn_right() {
@@ -167,6 +173,7 @@
 void DriveControl::turn_around() {
     // TODO: Add PID Control
     //pid_controller::turn(TURN_AROUND);
+    pid_controller::turn(true);
 }
 
 void DriveControl::stop() {
@@ -180,41 +187,34 @@
 }
 
 void DriveControl::drive_forward() {
-    /*
-    float COOL_DOWN = 1.0f;
-    if ((rightFrontIR.readIR() * 1000) > COOL_DOWN) {
-        pid_controller::cool_down_speed();
-    }
-    */
     pid_controller::navigate();
     
 }
 
 bool DriveControl::should_stop_drive_forward() {
     float SHOULD_STOP = 7.0f;
-    /*
-    if ((rightFrontIR.readIR() * 1000) > SHOULD_STOP) {
-        stop();
-        cool_down_offset = 0.0f;
-    }*/
     return (rightFrontIR.readIR() * 1000) > SHOULD_STOP;
 }
 
 bool DriveControl::should_finish_turn_right() {
-    // bool should_turn = (leftEncoder.getEncoderDistance(1) < -16000) || (rightEncoder.getEncoderDistance(0) < -16000);
-    int max = (leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0))
-        ? rightEncoder.getEncoderDistance(0) : leftEncoder.getEncoderDistance(1);
-    return max < -16000;
-    
-    //return should_turn;
+    int max = 
+        (leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0))
+        ? rightEncoder.getEncoderDistance(0):leftEncoder.getEncoderDistance(1);
+    return max < - ENCODER_TURN_UNIT;
 }
 
 bool DriveControl::should_finish_turn_left() {
-    int min_two = (leftEncoder.getEncoderDistance(1) > rightEncoder.getEncoderDistance(0))
-        ? rightEncoder.getEncoderDistance(0) : leftEncoder.getEncoderDistance(1);
-    //return leftEncoder.getEncoderDistance(1) > 15000 
-    //    || rightEncoder.getEncoderDistance(0) > 15000;
-    return min_two > 16000;
+    int min_two = 
+        (leftEncoder.getEncoderDistance(1) > rightEncoder.getEncoderDistance(0))
+        ? rightEncoder.getEncoderDistance(0):leftEncoder.getEncoderDistance(1);
+    return min_two > ENCODER_TURN_UNIT;
+}
+
+bool DriveControl::should_finish_drive_forward() {
+    int max_two = 
+        (- leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0))
+        ? rightEncoder.getEncoderDistance(0):- leftEncoder.getEncoderDistance(1);
+    return max_two > 46000;
 }
 
 bool DriveControl::has_front_wall() {
@@ -230,3 +230,7 @@
 bool DriveControl::has_right_wall() {
     return rightIR.readIR() > RIGHT_SIDE_WALL_THRES;
 }
+
+void DriveControl::clear_pid() {
+    pid_controller::clear_pid();
+}