TomYumBoys / Mbed 2 deprecated MM2017

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@2:619b02232144, 2017-05-13 (annotated)

Committer:
kolanery
Date:
Sat May 13 06:13:23 2017 +0000
Revision:
2:619b02232144
Parent:
0:cb667de3a336
Child:
3:4230b82fde43
update controller 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 2:619b02232144 4 #include "left_motor.h"
kolanery 2:619b02232144 5 #include "right_motor.h"
kolanery 0:cb667de3a336 6 //#include "pidconstants.h"
kolanery 0:cb667de3a336 7
kolanery 0:cb667de3a336 8
kolanery 0:cb667de3a336 9
kolanery 0:cb667de3a336 10 // Define states for debugging the mouse hardware
kolanery 0:cb667de3a336 11 const int DRIVE = 1, TURN = 2, DEBUG = 3, STOP = 4;
kolanery 0:cb667de3a336 12 const int SENSOR_THRESHOLD = 12;
kolanery 2:619b02232144 13 LeftMotor * leftMotor;
kolanery 2:619b02232144 14 RightMotor * rightMotor;
kolanery 0:cb667de3a336 15 Cell * curr_cell;
kolanery 0:cb667de3a336 16
kolanery 0:cb667de3a336 17 // Currently only have the x, y position fields for
kolanery 0:cb667de3a336 18 // each cell.
kolanery 0:cb667de3a336 19 DriveControl::DriveControl (int start_x, int start_y) {
kolanery 0:cb667de3a336 20 curr_cell = new Cell (start_x, start_y);
kolanery 2:619b02232144 21 leftMotor= new LeftMotor();
kolanery 2:619b02232144 22 rightMotor = new RightMotor();
kolanery 2:619b02232144 23 // leftMotor->speed(0.9f);
kolanery 2:619b02232144 24 // rightMotor->speed(0.9f);
kolanery 0:cb667de3a336 25 }
kolanery 0:cb667de3a336 26
kolanery 0:cb667de3a336 27 // Defines the next cell to traverse.
kolanery 0:cb667de3a336 28 Cell * next_cell() {
kolanery 0:cb667de3a336 29 // cell should get the reference from the Algorithm class.
kolanery 0:cb667de3a336 30 // Cell * cell;
kolanery 0:cb667de3a336 31 return curr_cell;
kolanery 0:cb667de3a336 32 }
kolanery 0:cb667de3a336 33
kolanery 0:cb667de3a336 34 void DriveControl::turn_left() {
kolanery 0:cb667de3a336 35
kolanery 0:cb667de3a336 36 // TODO: Add PID Control
kolanery 0:cb667de3a336 37 }
kolanery 0:cb667de3a336 38
kolanery 0:cb667de3a336 39 int DriveControl::get_next_direction() {
kolanery 0:cb667de3a336 40 // TODO: Define the direction based on heuristic eval.
kolanery 0:cb667de3a336 41 return 1;
kolanery 0:cb667de3a336 42 }
kolanery 0:cb667de3a336 43
kolanery 0:cb667de3a336 44 int DriveControl::get_next_state(int state) {
kolanery 2:619b02232144 45 // Simply drives the mouse for testing
kolanery 2:619b02232144 46 return DRIVE;
kolanery 2:619b02232144 47
kolanery 2:619b02232144 48 /*
kolanery 0:cb667de3a336 49 // Front wall threshold is set to 12
kolanery 0:cb667de3a336 50 if (this->DriveControl::has_front_wall()) {
kolanery 0:cb667de3a336 51 return DRIVE;
kolanery 0:cb667de3a336 52 }
kolanery 0:cb667de3a336 53
kolanery 2:619b02232144 54 if (!has_right_wall() || !has_left_wall()) {
kolanery 0:cb667de3a336 55 // return TURN;
kolanery 2:619b02232144 56 }
kolanery 0:cb667de3a336 57 // Add Another Check for abnormal state
kolanery 2:619b02232144 58 return DEBUG; */
kolanery 0:cb667de3a336 59 }
kolanery 0:cb667de3a336 60
kolanery 0:cb667de3a336 61 void DriveControl::turn_right() {
kolanery 0:cb667de3a336 62
kolanery 0:cb667de3a336 63 // TODO: Add PID Control
kolanery 0:cb667de3a336 64 }
kolanery 0:cb667de3a336 65
kolanery 0:cb667de3a336 66 void DriveControl::stop() {
kolanery 2:619b02232144 67 leftMotor->stop();
kolanery 2:619b02232144 68 rightMotor->stop();
kolanery 2:619b02232144 69
kolanery 0:cb667de3a336 70 }
kolanery 0:cb667de3a336 71
kolanery 0:cb667de3a336 72 void DriveControl::drive_one_forward() {
kolanery 0:cb667de3a336 73 // TODO: Add PID Control
kolanery 0:cb667de3a336 74 //boolean stopLoop = false;
kolanery 0:cb667de3a336 75 //while (!stopLoop) {
kolanery 0:cb667de3a336 76 //}
kolanery 2:619b02232144 77 //int curr_dist = 0;
kolanery 2:619b02232144 78 //while (curr_dist != 5) {
kolanery 2:619b02232144 79 leftMotor->speed(0.2f);
kolanery 2:619b02232144 80 rightMotor->speed(0.8f);
kolanery 2:619b02232144 81 // curr_dist ++;
kolanery 2:619b02232144 82 //}
kolanery 2:619b02232144 83 //curr_dist = 0;
kolanery 0:cb667de3a336 84 }
kolanery 0:cb667de3a336 85
kolanery 0:cb667de3a336 86 bool DriveControl::has_front_wall() {
kolanery 0:cb667de3a336 87 return rightFrontIR < SENSOR_THRESHOLD && leftFrontIR < SENSOR_THRESHOLD;
kolanery 0:cb667de3a336 88 }
kolanery 0:cb667de3a336 89
kolanery 0:cb667de3a336 90 bool DriveControl::has_left_wall() {
kolanery 0:cb667de3a336 91 return leftIR < SENSOR_THRESHOLD;
kolanery 0:cb667de3a336 92 }
kolanery 0:cb667de3a336 93
kolanery 0:cb667de3a336 94 bool DriveControl::has_right_wall() {
kolanery 0:cb667de3a336 95 return rightIR < SENSOR_THRESHOLD;
kolanery 0:cb667de3a336 96 }
kolanery 0:cb667de3a336 97