Diff: Control/drivecontrol.cpp

Revision:

18:85196734207e

Parent:

17:043ed1d0196f

--- a/Control/drivecontrol.cpp	Sat May 20 21:34:06 2017 +0000
+++ b/Control/drivecontrol.cpp	Sat May 20 22:37:18 2017 +0000
@@ -80,8 +80,6 @@
             //leftMotor->speed(MOTOR_BASE_SPEED);
             //rightMotor->speed(MOTOR_BASE_SPEED);
         }
-        
-        
     }
     
     void clear_pid(){
@@ -95,24 +93,6 @@
         }
     }
     
-    void one_cell_turned(){
-        leftEncoder.reset();
-        rightEncoder.reset();
-        while(leftEncoder.getEncoderDistance(1)<-46000 & leftEncoder.getEncoderDistance(1)<46000){
-            //Do nothing
-        }
-    }
-    
-    // should use this method as the exit condition
-    // in the pid_controller::navigate() method
-    // resets the pid, encoders, etc.
-    void one_cell_traversed() {
-        leftEncoder.reset();
-        rightEncoder.reset();
-        old_error_p = old_error_d = total_error = 0.0f;
-    }
-    
-    // TODO
     void turn (bool turn_right) {
         float MOTOR_TURN_SPEED = 14.0f;
         
@@ -150,6 +130,8 @@
     //return DRIVE;
 }
 
+
+
 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));
@@ -161,6 +143,19 @@
     pc.printf("RIGHT SIDE Sensor %f\n\r", rightIR.readIR());
 }
 
+
+bool DriveControl::should_reverse() {
+    if (abs(leftEncoder.getEncoderDistance(1)) + abs(rightEncoder.getEncoderDistance(0)) < 28000) {
+        return true;
+    }
+    return false;
+}
+
+void DriveControl::reverse () {
+    leftMotor -> speed(- MOTOR_BASE_SPEED);
+    rightMotor -> speed(- MOTOR_BASE_SPEED);
+}
+
 void DriveControl::turn_left() {
     // TODO: Add PID Control
     pid_controller::turn(false);
@@ -171,11 +166,11 @@
     pid_controller::turn(true);
 }
 
-void DriveControl::turn_around() {
-    // TODO: Add PID Control
-    //pid_controller::turn(TURN_AROUND);
-    pid_controller::turn(true);
-}
+//void DriveControl::turn_around() {
+//    // TODO: Add PID Control
+//    //pid_controller::turn(TURN_AROUND);
+//    pid_controller::turn(true);
+//}
 
 void DriveControl::stop() {
     leftMotor->stop();