Diff: Control/drivecontrol.cpp

Revision:

4:73510c7fa316

Parent:

3:4230b82fde43

Child:

5:d783972dce10

--- a/Control/drivecontrol.cpp	Sat May 13 06:22:19 2017 +0000
+++ b/Control/drivecontrol.cpp	Sat May 13 21:27:32 2017 +0000
@@ -3,25 +3,60 @@
 #include "ir_sensor.h"
 #include "left_motor.h"
 #include "right_motor.h"
-//#include "pidconstants.h"
 
-
-
+// Bit to specify debugging mode
+const bool DEBUG = false;
 // Define states for debugging the mouse hardware
-const int DRIVE = 1, TURN = 2, DEBUG = 3, STOP = 4;
+const int DRIVE = 1, TURN = 2, STOP = 4;
 const int SENSOR_THRESHOLD = 12;
 LeftMotor * leftMotor;
 RightMotor * rightMotor;
 Cell * curr_cell;
 
+namespace pid_controller
+{
+    float motor_speed;
+    float LEFT_WALL_THRES = 0.89f, RIGHT_WALL_THRES = 0.91f;
+    float SENSOR_ERROR_OFFSET = 0.02f;
+    
+    float error_p = 0.0f, old_error_p = 0.0f, error_d = 0.0f;
+    float total_error;
+    float P = 0.02f, D = 0.01f;
+    bool has_left_wall = leftIR.readIR() > LEFT_WALL_THRES;
+    bool has_right_wall = rightIR.readIR() > RIGHT_WALL_THRES;
+    
+    
+    void start_pid() {
+        if (has_left_wall && has_right_wall) {
+            error_p = rightIR - leftIR - SENSOR_ERROR_OFFSET;
+            error_d = error_p - old_error_p;    
+        }
+        else if (has_left_wall) {
+            error_p = 2 * (LEFT_WALL_THRES - leftIR.readIR());
+            error_d = error_p - old_error_p;
+        }
+        else if (has_right_wall) {
+            error_p = 2 * (rightIR.readIR() - RIGHT_WALL_THRES);
+            error_d = error_p - old_error_p;
+        }
+        else if (!has_left_wall && !has_right_wall) {
+            error_p = 0;
+            error_d = 0;
+        }
+        total_error = P * error_p + D*error_d;
+        old_error_p = error_p;
+        // if (DEBUG) { leftMotor->speed(0.15f); rightMotor->speed(0.85f); }
+        leftMotor -> speed(0.15f - total_error);
+        rightMotor -> speed(0.85f + total_error);
+    }    
+}
+
 // Currently only have the x, y position fields for
 // each cell.
 DriveControl::DriveControl (int start_x, int start_y) {
     curr_cell = new Cell (start_x, start_y);
     leftMotor= new LeftMotor();
     rightMotor = new RightMotor();
- //   leftMotor->speed(0.9f);
-//    rightMotor->speed(0.9f);
 }
 
 // Defines the next cell to traverse.
@@ -32,7 +67,6 @@
 }
 
 void DriveControl::turn_left() {
-    
     // TODO: Add PID Control
 }
 
@@ -59,28 +93,28 @@
 }
 
 void DriveControl::turn_right() {
+    // TODO: Add PID Control
     
-    // TODO: Add PID Control
 }
 
 void DriveControl::stop() {
     leftMotor->stop();
     rightMotor->stop(); 
-       
 }
 
 void DriveControl::drive_one_forward() {
-    // TODO: Add PID Control
-    //boolean stopLoop = false;
-    //while (!stopLoop) {
-    //}
-    //int curr_dist = 0;
-    //while (curr_dist != 5) {
-        leftMotor->speed(0.15f);
-        rightMotor->speed(0.85f);
-    //    curr_dist ++;
-    //}
-    //curr_dist = 0;
+    if (DEBUG) { leftMotor->speed(0.15f); rightMotor->speed(0.85f); }
+    else {
+        int count = 0;
+        while (count != 10) {
+            pid_controller::start_pid();
+            count ++;
+        }
+    }
+}
+
+void start_pid() {
+    
 }
 
 bool DriveControl::has_front_wall() {
@@ -94,4 +128,3 @@
 bool DriveControl::has_right_wall() {
     return rightIR < SENSOR_THRESHOLD;
 }
-