Diff: Control/drivecontrol.cpp

Revision:

13:a1a3418c07f3

Parent:

12:6f48afe41cd9

Child:

14:a646667ac9ea

--- a/Control/drivecontrol.cpp	Wed May 17 00:18:14 2017 +0000
+++ b/Control/drivecontrol.cpp	Wed May 17 07:27:46 2017 +0000
@@ -4,39 +4,35 @@
 #include "left_motor.h"
 #include "right_motor.h"
 #include "pin_assignment.h"
-//#include "encoder.h"
 
 IRSensor leftIR(PA_8, PC_5);
 IRSensor rightIR(PB_0, PA_4);
-
 //Encoder rightEncoder(PA_1, PA_0);
 //Encoder leftEncoder(PC_9, PC_8);
-
-// Define states for debugging the mouse hardware
-const int DRIVE = 0, TURN = 2, STOP = 4;
-const int TURN_LEFT = 0, TURN_RIGHT = 1, TURN_AROUND = 2;
-
 LeftMotor * leftMotor;
 RightMotor * rightMotor;
 Cell * curr_cell;
 Serial pc(PA_9, PA_10);
 
-float motor_speed;
+// Define states for debugging the mouse hardware
+const int DRIVE = 0; //, TURN = 2, STOP = 4;
+const int TURN_LEFT = 0, TURN_RIGHT = 1, TURN_AROUND = 2;
+
+// Sensor offsets
+float FRONT_SENSOR_THRESHOLD = 0.90f, SENSOR_ERROR_OFFSET = 0.0f;
+float LEFT_WALL_THRES = 0.46f, RIGHT_WALL_THRES = 0.21f;
+const int SENSOR_THRESHOLD = 12;
+
+// Motor speed offsets
+float left_speed, right_speed, motor_speed;
 float MOTOR_BASE_SPEED = 15.0f;
 
-float FRONT_SENSOR_THRESHOLD = 0.90f;
-const int SENSOR_THRESHOLD = 12;
-float LEFT_WALL_THRES = 0.46f, RIGHT_WALL_THRES = 0.21f;
-float SENSOR_ERROR_OFFSET = 0.0f;
-
-
-float left_speed;
-float right_speed;
-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;
-
-namespace pid_controller {
+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;
+    
     void navigate() {
         bool has_left_wall = leftDiagonalIR.readIR() > LEFT_WALL_THRES;
         bool has_right_wall = rightDiagonalIR.readIR() > RIGHT_WALL_THRES;
@@ -78,11 +74,9 @@
             leftMotor->speed(MOTOR_BASE_SPEED);
             rightMotor->speed(MOTOR_BASE_SPEED);
         }
-        
-        one_cell();
     }
     
-    void one_cell(){
+    void one_cell_turned(){
         leftEncoder.reset();
         rightEncoder.reset();
         while(leftEncoder.getEncoderDistance(1)<-46000 & leftEncoder.getEncoderDistance(1)<46000){
@@ -90,10 +84,19 @@
         }
     }
     
-    // todo
+    // 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 (int dir) {
         // https://github.com/austinxiao-ucsd/Falcon-MicroMouse/blob/master/Micromouse_test/drive_control.h
-        if (TURN_LEFT) { // Flip motor 
+        if (TURN_LEFT) { // Flip motor speed to do in place turning 
         }
         else { // TODO: ...
         }