Diff: Control/drivecontrol.cpp

Revision:

0:cb667de3a336

Child:

2:619b02232144

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/drivecontrol.cpp	Sat May 06 19:34:26 2017 +0000
@@ -0,0 +1,94 @@
+#include "drivecontrol.h"
+#include "Cell.h"
+#include "ir_sensor.h"
+//#include "pidconstants.h"
+
+// Constants for PID control
+#define POS_TURN_CONST  0.5f
+#define NEG_TURN_CONST -0.5f;
+// 1024 * (40/8) * (18 / (3.1415*1.3))   
+//encoder_units_per_revolution * gear_ratio * (cell_size / wheel_circumference)
+#define CELL_DIST 10600;
+
+#define PID_SAMPLE_PERIOD 0.005 // 5 ms
+#define CELL_DISTANCE   10600
+#define DISTANCE_CONST_P    22
+#define DISTANCE_CONST_D    43
+#define DISTANCE_CONST_I    10000
+
+
+// Define states for debugging the mouse hardware
+const int DRIVE = 1, TURN = 2, DEBUG = 3, STOP = 4;
+const int SENSOR_THRESHOLD = 12;
+
+Cell * curr_cell;
+
+// 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);
+}
+
+// Defines the next cell to traverse.
+Cell * next_cell() {
+    // cell should get the reference from the Algorithm class.
+    // Cell * cell;
+    return curr_cell;
+}
+
+void DriveControl::turn_left() {
+    leftMotor = NEG_TURN_CONST;
+    rightMotor = POS_TURN_CONST;
+    
+    // TODO: Add PID Control
+}
+
+int DriveControl::get_next_direction() {
+    // TODO: Define the direction based on heuristic eval. 
+    return 1;   
+}
+
+int DriveControl::get_next_state(int state) {
+    // Front wall threshold is set to 12
+    if (this->DriveControl::has_front_wall()) {
+        return DRIVE;
+    }
+    
+    //if (!has_right_wall() || !has_left_wall()) {
+     //   return TURN;
+    //}
+    // Add Another Check for abnormal state
+    return DEBUG;    
+}
+
+void DriveControl::turn_right() {
+    leftMotor = POS_TURN_CONST;
+    rightMotor = NEG_TURN_CONST;
+    
+    // TODO: Add PID Control
+}
+
+void DriveControl::stop() {
+    leftMotor = 0;
+    rightMotor = 0;    
+}
+
+void DriveControl::drive_one_forward() {
+    // TODO: Add PID Control
+    //boolean stopLoop = false;
+    //while (!stopLoop) {
+    //}
+}
+
+bool DriveControl::has_front_wall() {
+    return rightFrontIR < SENSOR_THRESHOLD && leftFrontIR < SENSOR_THRESHOLD;
+}
+
+bool DriveControl::has_left_wall() {
+    return leftIR < SENSOR_THRESHOLD;
+}
+
+bool DriveControl::has_right_wall() {
+    return rightIR < SENSOR_THRESHOLD;
+}
+