TomYumBoys / Mbed 2 deprecated MM2017

Dependencies:   mbed

Committer:
kolanery
Date:
Sat May 06 19:34:26 2017 +0000
Revision:
0:cb667de3a336
Child:
2:619b02232144
update baseline code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kolanery 0:cb667de3a336 1 #include "drivecontrol.h"
kolanery 0:cb667de3a336 2 #include "Cell.h"
kolanery 0:cb667de3a336 3 #include "ir_sensor.h"
kolanery 0:cb667de3a336 4 //#include "pidconstants.h"
kolanery 0:cb667de3a336 5
kolanery 0:cb667de3a336 6 // Constants for PID control
kolanery 0:cb667de3a336 7 #define POS_TURN_CONST 0.5f
kolanery 0:cb667de3a336 8 #define NEG_TURN_CONST -0.5f;
kolanery 0:cb667de3a336 9 // 1024 * (40/8) * (18 / (3.1415*1.3))
kolanery 0:cb667de3a336 10 //encoder_units_per_revolution * gear_ratio * (cell_size / wheel_circumference)
kolanery 0:cb667de3a336 11 #define CELL_DIST 10600;
kolanery 0:cb667de3a336 12
kolanery 0:cb667de3a336 13 #define PID_SAMPLE_PERIOD 0.005 // 5 ms
kolanery 0:cb667de3a336 14 #define CELL_DISTANCE 10600
kolanery 0:cb667de3a336 15 #define DISTANCE_CONST_P 22
kolanery 0:cb667de3a336 16 #define DISTANCE_CONST_D 43
kolanery 0:cb667de3a336 17 #define DISTANCE_CONST_I 10000
kolanery 0:cb667de3a336 18
kolanery 0:cb667de3a336 19
kolanery 0:cb667de3a336 20 // Define states for debugging the mouse hardware
kolanery 0:cb667de3a336 21 const int DRIVE = 1, TURN = 2, DEBUG = 3, STOP = 4;
kolanery 0:cb667de3a336 22 const int SENSOR_THRESHOLD = 12;
kolanery 0:cb667de3a336 23
kolanery 0:cb667de3a336 24 Cell * curr_cell;
kolanery 0:cb667de3a336 25
kolanery 0:cb667de3a336 26 // Currently only have the x, y position fields for
kolanery 0:cb667de3a336 27 // each cell.
kolanery 0:cb667de3a336 28 DriveControl::DriveControl (int start_x, int start_y) {
kolanery 0:cb667de3a336 29 curr_cell = new Cell (start_x, start_y);
kolanery 0:cb667de3a336 30 }
kolanery 0:cb667de3a336 31
kolanery 0:cb667de3a336 32 // Defines the next cell to traverse.
kolanery 0:cb667de3a336 33 Cell * next_cell() {
kolanery 0:cb667de3a336 34 // cell should get the reference from the Algorithm class.
kolanery 0:cb667de3a336 35 // Cell * cell;
kolanery 0:cb667de3a336 36 return curr_cell;
kolanery 0:cb667de3a336 37 }
kolanery 0:cb667de3a336 38
kolanery 0:cb667de3a336 39 void DriveControl::turn_left() {
kolanery 0:cb667de3a336 40 leftMotor = NEG_TURN_CONST;
kolanery 0:cb667de3a336 41 rightMotor = POS_TURN_CONST;
kolanery 0:cb667de3a336 42
kolanery 0:cb667de3a336 43 // TODO: Add PID Control
kolanery 0:cb667de3a336 44 }
kolanery 0:cb667de3a336 45
kolanery 0:cb667de3a336 46 int DriveControl::get_next_direction() {
kolanery 0:cb667de3a336 47 // TODO: Define the direction based on heuristic eval.
kolanery 0:cb667de3a336 48 return 1;
kolanery 0:cb667de3a336 49 }
kolanery 0:cb667de3a336 50
kolanery 0:cb667de3a336 51 int DriveControl::get_next_state(int state) {
kolanery 0:cb667de3a336 52 // Front wall threshold is set to 12
kolanery 0:cb667de3a336 53 if (this->DriveControl::has_front_wall()) {
kolanery 0:cb667de3a336 54 return DRIVE;
kolanery 0:cb667de3a336 55 }
kolanery 0:cb667de3a336 56
kolanery 0:cb667de3a336 57 //if (!has_right_wall() || !has_left_wall()) {
kolanery 0:cb667de3a336 58 // return TURN;
kolanery 0:cb667de3a336 59 //}
kolanery 0:cb667de3a336 60 // Add Another Check for abnormal state
kolanery 0:cb667de3a336 61 return DEBUG;
kolanery 0:cb667de3a336 62 }
kolanery 0:cb667de3a336 63
kolanery 0:cb667de3a336 64 void DriveControl::turn_right() {
kolanery 0:cb667de3a336 65 leftMotor = POS_TURN_CONST;
kolanery 0:cb667de3a336 66 rightMotor = NEG_TURN_CONST;
kolanery 0:cb667de3a336 67
kolanery 0:cb667de3a336 68 // TODO: Add PID Control
kolanery 0:cb667de3a336 69 }
kolanery 0:cb667de3a336 70
kolanery 0:cb667de3a336 71 void DriveControl::stop() {
kolanery 0:cb667de3a336 72 leftMotor = 0;
kolanery 0:cb667de3a336 73 rightMotor = 0;
kolanery 0:cb667de3a336 74 }
kolanery 0:cb667de3a336 75
kolanery 0:cb667de3a336 76 void DriveControl::drive_one_forward() {
kolanery 0:cb667de3a336 77 // TODO: Add PID Control
kolanery 0:cb667de3a336 78 //boolean stopLoop = false;
kolanery 0:cb667de3a336 79 //while (!stopLoop) {
kolanery 0:cb667de3a336 80 //}
kolanery 0:cb667de3a336 81 }
kolanery 0:cb667de3a336 82
kolanery 0:cb667de3a336 83 bool DriveControl::has_front_wall() {
kolanery 0:cb667de3a336 84 return rightFrontIR < SENSOR_THRESHOLD && leftFrontIR < SENSOR_THRESHOLD;
kolanery 0:cb667de3a336 85 }
kolanery 0:cb667de3a336 86
kolanery 0:cb667de3a336 87 bool DriveControl::has_left_wall() {
kolanery 0:cb667de3a336 88 return leftIR < SENSOR_THRESHOLD;
kolanery 0:cb667de3a336 89 }
kolanery 0:cb667de3a336 90
kolanery 0:cb667de3a336 91 bool DriveControl::has_right_wall() {
kolanery 0:cb667de3a336 92 return rightIR < SENSOR_THRESHOLD;
kolanery 0:cb667de3a336 93 }
kolanery 0:cb667de3a336 94