Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Control/drivecontrol.cpp@0:cb667de3a336, 2017-05-06 (annotated)
- 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?
User | Revision | Line number | New 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 |