Diff: Control/drivecontrol.cpp

Revision:

14:a646667ac9ea

Parent:

13:a1a3418c07f3

Child:

15:151e59899221

--- a/Control/drivecontrol.cpp	Wed May 17 07:27:46 2017 +0000
+++ b/Control/drivecontrol.cpp	Fri May 19 16:46:21 2017 +0000
@@ -20,18 +20,21 @@
 
 // Sensor offsets
 float FRONT_SENSOR_THRESHOLD = 0.90f, SENSOR_ERROR_OFFSET = 0.0f;
-float LEFT_WALL_THRES = 0.46f, RIGHT_WALL_THRES = 0.21f;
+//float LEFT_WALL_THRES = 0.46f, RIGHT_WALL_THRES = 0.21f;
+float LEFT_WALL_THRES = 0.552f, RIGHT_WALL_THRES = 0.198f;
 const int SENSOR_THRESHOLD = 12;
+float SENSOR_RIGHT_WALL_THRES = 0.3f, SENSOR_LEFT_WALL_THRES = 0.3f;
 
 // Motor speed offsets
 float left_speed, right_speed, motor_speed;
-float MOTOR_BASE_SPEED = 15.0f;
+float MOTOR_BASE_SPEED = 10.0f;
+float cool_down_offset = 0.0f;
 
 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 = 3.0f;
+    float P = 18.0f, D = 5.0f;//3.0f;
     
     void navigate() {
         bool has_left_wall = leftDiagonalIR.readIR() > LEFT_WALL_THRES;
@@ -65,14 +68,27 @@
         old_error_p = error_p;
         old_error_d = error_d;
         
-        
-        if(total_error < 7.5f){
+        //MOTOR_BASE_SPEED -= cool_down_offset;
+        if(total_error < 5.0f){
             leftMotor -> speed(MOTOR_BASE_SPEED - total_error);
             rightMotor -> speed(MOTOR_BASE_SPEED + total_error);
+            
+            //leftMotor -> speed(MOTOR_BASE_SPEED - total_error);
+            //rightMotor -> speed(MOTOR_BASE_SPEED + total_error);
         }
         else{
             leftMotor->speed(MOTOR_BASE_SPEED);
             rightMotor->speed(MOTOR_BASE_SPEED);
+            //leftMotor->speed(MOTOR_BASE_SPEED);
+            //rightMotor->speed(MOTOR_BASE_SPEED);
+        }
+        
+        
+    }
+    
+    void cool_down_speed() {
+        if (cool_down_offset < 5.0f) {
+            cool_down_offset += 2.0f;
         }
     }
     
@@ -94,11 +110,16 @@
     }
     
     // TODO
-    void turn (int dir) {
+    void turn (bool turn_right) {
+        float MOTOR_TURN_SPEED = 15.0f;
         // https://github.com/austinxiao-ucsd/Falcon-MicroMouse/blob/master/Micromouse_test/drive_control.h
-        if (TURN_LEFT) { // Flip motor speed to do in place turning 
+        if (turn_right) { // Flip motor speed to do in place turning 
+            leftMotor -> speed(MOTOR_TURN_SPEED);
+            rightMotor -> speed(- MOTOR_TURN_SPEED);
         }
-        else { // TODO: ...
+        else {
+            leftMotor -> speed(- MOTOR_TURN_SPEED);
+            rightMotor -> speed(MOTOR_TURN_SPEED);
         }
     }
 }
@@ -118,7 +139,7 @@
 
 void DriveControl::turn_left() {
     // TODO: Add PID Control
-    pid_controller::turn(TURN_LEFT);
+    pid_controller::turn(false);
 }
 
 int DriveControl::get_next_direction() {
@@ -132,13 +153,19 @@
 }
 
 void DriveControl::getEncoder(){
-    pc.printf("LeftEncoder Reading %d", leftEncoder.getEncoderDistance(1));
-    pc.printf("RightEncoder Reading %d\n\r", rightEncoder.getEncoderDistance(0));
+//    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() * 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());
+    pc.printf("RIGHT SIDE Sensor %f\n\r", rightIR.readIR());
 }
 
 void DriveControl::turn_right() {
     // TODO: Add PID Control
-    pid_controller::turn(TURN_RIGHT);
+    pid_controller::turn(true);
 }
 
 void DriveControl::turn_around() {
@@ -151,20 +178,54 @@
     rightMotor->stop(); 
 }
 
+void DriveControl::resetEncoders() {
+    leftEncoder.resetEncoders();
+    rightEncoder.resetEncoders();
+}
+
 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);
+    return should_turn;
+}
+
+bool DriveControl::should_finish_turn_left() {
+    return leftEncoder.getEncoderDistance(1) > 16000 
+        && rightEncoder.getEncoderDistance(0) > 16000;
 }
 
 
 // TODO: Test top right ir sensor 
 bool DriveControl::has_front_wall() {
-    return rightFrontIR > FRONT_SENSOR_THRESHOLD;
+    //return rightFrontIR > FRONT_SENSOR_THRESHOLD;
+    return (rightFrontIR.readIR() * 1000) > 7.0f;
 }
 
 bool DriveControl::has_left_wall() {
-    return leftIR < SENSOR_THRESHOLD;
+    return leftIR.readIR() > SENSOR_LEFT_WALL_THRES;
 }
 
 bool DriveControl::has_right_wall() {
-    return rightIR < SENSOR_THRESHOLD;
+    return rightIR.readIR() > SENSOR_RIGHT_WALL_THRES;
 }