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@14:a646667ac9ea, 2017-05-19 (annotated)

Committer:
kolanery
Date:
Fri May 19 16:46:21 2017 +0000
Revision:
14:a646667ac9ea
Parent:
13:a1a3418c07f3
Child:
15:151e59899221
add wall follower

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"
szh66 6:1bcfda49e146 6 #include "pin_assignment.h"
kolanery 0:cb667de3a336 7
szh66 6:1bcfda49e146 8 IRSensor leftIR(PA_8, PC_5);
szh66 6:1bcfda49e146 9 IRSensor rightIR(PB_0, PA_4);
szh66 8:4a32fc9ee939 10 //Encoder rightEncoder(PA_1, PA_0);
szh66 8:4a32fc9ee939 11 //Encoder leftEncoder(PC_9, PC_8);
kolanery 2:619b02232144 12 LeftMotor * leftMotor;
kolanery 2:619b02232144 13 RightMotor * rightMotor;
kolanery 0:cb667de3a336 14 Cell * curr_cell;
szh66 6:1bcfda49e146 15 Serial pc(PA_9, PA_10);
szh66 6:1bcfda49e146 16
kolanery 13:a1a3418c07f3 17 // Define states for debugging the mouse hardware
kolanery 13:a1a3418c07f3 18 const int DRIVE = 0; //, TURN = 2, STOP = 4;
kolanery 13:a1a3418c07f3 19 const int TURN_LEFT = 0, TURN_RIGHT = 1, TURN_AROUND = 2;
kolanery 13:a1a3418c07f3 20
kolanery 13:a1a3418c07f3 21 // Sensor offsets
kolanery 13:a1a3418c07f3 22 float FRONT_SENSOR_THRESHOLD = 0.90f, SENSOR_ERROR_OFFSET = 0.0f;
kolanery 14:a646667ac9ea 23 //float LEFT_WALL_THRES = 0.46f, RIGHT_WALL_THRES = 0.21f;
kolanery 14:a646667ac9ea 24 float LEFT_WALL_THRES = 0.552f, RIGHT_WALL_THRES = 0.198f;
kolanery 13:a1a3418c07f3 25 const int SENSOR_THRESHOLD = 12;
kolanery 14:a646667ac9ea 26 float SENSOR_RIGHT_WALL_THRES = 0.3f, SENSOR_LEFT_WALL_THRES = 0.3f;
kolanery 13:a1a3418c07f3 27
kolanery 13:a1a3418c07f3 28 // Motor speed offsets
kolanery 13:a1a3418c07f3 29 float left_speed, right_speed, motor_speed;
kolanery 14:a646667ac9ea 30 float MOTOR_BASE_SPEED = 10.0f;
kolanery 14:a646667ac9ea 31 float cool_down_offset = 0.0f;
kolanery 7:7215adbae3da 32
kolanery 13:a1a3418c07f3 33 namespace pid_controller {
kolanery 13:a1a3418c07f3 34 // PID Constants
kolanery 13:a1a3418c07f3 35 float error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f;
kolanery 13:a1a3418c07f3 36 float total_error = 0.0f;
kolanery 14:a646667ac9ea 37 float P = 18.0f, D = 5.0f;//3.0f;
kolanery 13:a1a3418c07f3 38
kolanery 7:7215adbae3da 39 void navigate() {
kolanery 7:7215adbae3da 40 bool has_left_wall = leftDiagonalIR.readIR() > LEFT_WALL_THRES;
kolanery 7:7215adbae3da 41 bool has_right_wall = rightDiagonalIR.readIR() > RIGHT_WALL_THRES;
kolanery 7:7215adbae3da 42
kolanery 7:7215adbae3da 43 if (has_left_wall && has_right_wall) {
kolanery 7:7215adbae3da 44 if (rightDiagonalIR - RIGHT_WALL_THRES < 0) {
kolanery 7:7215adbae3da 45 error_p = rightDiagonalIR - RIGHT_WALL_THRES;
kolanery 7:7215adbae3da 46 }
kolanery 7:7215adbae3da 47 else if (leftDiagonalIR - LEFT_WALL_THRES < 0) {
kolanery 7:7215adbae3da 48 error_p = leftDiagonalIR - LEFT_WALL_THRES;
kolanery 7:7215adbae3da 49 }
kolanery 7:7215adbae3da 50 else{
kolanery 7:7215adbae3da 51 error_p = rightDiagonalIR - leftDiagonalIR;
kolanery 7:7215adbae3da 52 }
kolanery 7:7215adbae3da 53 error_d = error_p - old_error_p;
kolanery 7:7215adbae3da 54 }
kolanery 7:7215adbae3da 55 else if (has_left_wall) {
kolanery 7:7215adbae3da 56 error_p = 2 * (LEFT_WALL_THRES - leftDiagonalIR.readIR());
kolanery 7:7215adbae3da 57 error_d = error_p - old_error_p;
kolanery 7:7215adbae3da 58 }
kolanery 7:7215adbae3da 59 else if (has_right_wall) {
kolanery 7:7215adbae3da 60 error_p = 2 * (rightDiagonalIR.readIR() - RIGHT_WALL_THRES);
kolanery 7:7215adbae3da 61 error_d = error_p - old_error_p;
kolanery 7:7215adbae3da 62 }
kolanery 7:7215adbae3da 63 else if (!has_left_wall && !has_right_wall) {
kolanery 7:7215adbae3da 64 error_p = 0;
kolanery 7:7215adbae3da 65 error_d = 0;
kolanery 7:7215adbae3da 66 }
kolanery 7:7215adbae3da 67 total_error = P * error_p + D * (error_d - old_error_d);
kolanery 7:7215adbae3da 68 old_error_p = error_p;
kolanery 7:7215adbae3da 69 old_error_d = error_d;
kolanery 7:7215adbae3da 70
kolanery 14:a646667ac9ea 71 //MOTOR_BASE_SPEED -= cool_down_offset;
kolanery 14:a646667ac9ea 72 if(total_error < 5.0f){
kolanery 7:7215adbae3da 73 leftMotor -> speed(MOTOR_BASE_SPEED - total_error);
kolanery 7:7215adbae3da 74 rightMotor -> speed(MOTOR_BASE_SPEED + total_error);
kolanery 14:a646667ac9ea 75
kolanery 14:a646667ac9ea 76 //leftMotor -> speed(MOTOR_BASE_SPEED - total_error);
kolanery 14:a646667ac9ea 77 //rightMotor -> speed(MOTOR_BASE_SPEED + total_error);
kolanery 7:7215adbae3da 78 }
kolanery 7:7215adbae3da 79 else{
kolanery 7:7215adbae3da 80 leftMotor->speed(MOTOR_BASE_SPEED);
kolanery 7:7215adbae3da 81 rightMotor->speed(MOTOR_BASE_SPEED);
kolanery 14:a646667ac9ea 82 //leftMotor->speed(MOTOR_BASE_SPEED);
kolanery 14:a646667ac9ea 83 //rightMotor->speed(MOTOR_BASE_SPEED);
kolanery 14:a646667ac9ea 84 }
kolanery 14:a646667ac9ea 85
kolanery 14:a646667ac9ea 86
kolanery 14:a646667ac9ea 87 }
kolanery 14:a646667ac9ea 88
kolanery 14:a646667ac9ea 89 void cool_down_speed() {
kolanery 14:a646667ac9ea 90 if (cool_down_offset < 5.0f) {
kolanery 14:a646667ac9ea 91 cool_down_offset += 2.0f;
kolanery 7:7215adbae3da 92 }
ryan_whr 12:6f48afe41cd9 93 }
ryan_whr 12:6f48afe41cd9 94
kolanery 13:a1a3418c07f3 95 void one_cell_turned(){
ryan_whr 12:6f48afe41cd9 96 leftEncoder.reset();
ryan_whr 12:6f48afe41cd9 97 rightEncoder.reset();
ryan_whr 12:6f48afe41cd9 98 while(leftEncoder.getEncoderDistance(1)<-46000 & leftEncoder.getEncoderDistance(1)<46000){
ryan_whr 12:6f48afe41cd9 99 //Do nothing
ryan_whr 12:6f48afe41cd9 100 }
kolanery 7:7215adbae3da 101 }
kolanery 7:7215adbae3da 102
kolanery 13:a1a3418c07f3 103 // should use this method as the exit condition
kolanery 13:a1a3418c07f3 104 // in the pid_controller::navigate() method
kolanery 13:a1a3418c07f3 105 // resets the pid, encoders, etc.
kolanery 13:a1a3418c07f3 106 void one_cell_traversed() {
kolanery 13:a1a3418c07f3 107 leftEncoder.reset();
kolanery 13:a1a3418c07f3 108 rightEncoder.reset();
kolanery 13:a1a3418c07f3 109 old_error_p = old_error_d = total_error = 0.0f;
kolanery 13:a1a3418c07f3 110 }
kolanery 13:a1a3418c07f3 111
kolanery 13:a1a3418c07f3 112 // TODO
kolanery 14:a646667ac9ea 113 void turn (bool turn_right) {
kolanery 14:a646667ac9ea 114 float MOTOR_TURN_SPEED = 15.0f;
kolanery 7:7215adbae3da 115 // https://github.com/austinxiao-ucsd/Falcon-MicroMouse/blob/master/Micromouse_test/drive_control.h
kolanery 14:a646667ac9ea 116 if (turn_right) { // Flip motor speed to do in place turning
kolanery 14:a646667ac9ea 117 leftMotor -> speed(MOTOR_TURN_SPEED);
kolanery 14:a646667ac9ea 118 rightMotor -> speed(- MOTOR_TURN_SPEED);
kolanery 7:7215adbae3da 119 }
kolanery 14:a646667ac9ea 120 else {
kolanery 14:a646667ac9ea 121 leftMotor -> speed(- MOTOR_TURN_SPEED);
kolanery 14:a646667ac9ea 122 rightMotor -> speed(MOTOR_TURN_SPEED);
kolanery 7:7215adbae3da 123 }
kolanery 7:7215adbae3da 124 }
kolanery 7:7215adbae3da 125 }
kolanery 7:7215adbae3da 126
kolanery 0:cb667de3a336 127 // Currently only have the x, y position fields for
kolanery 0:cb667de3a336 128 // each cell.
kolanery 0:cb667de3a336 129 DriveControl::DriveControl (int start_x, int start_y) {
kolanery 0:cb667de3a336 130 curr_cell = new Cell (start_x, start_y);
kolanery 2:619b02232144 131 leftMotor= new LeftMotor();
kolanery 2:619b02232144 132 rightMotor = new RightMotor();
kolanery 0:cb667de3a336 133 }
kolanery 0:cb667de3a336 134
kolanery 0:cb667de3a336 135 // Defines the next cell to traverse.
kolanery 0:cb667de3a336 136 Cell * next_cell() {
kolanery 0:cb667de3a336 137 return curr_cell;
kolanery 0:cb667de3a336 138 }
kolanery 0:cb667de3a336 139
kolanery 0:cb667de3a336 140 void DriveControl::turn_left() {
kolanery 0:cb667de3a336 141 // TODO: Add PID Control
kolanery 14:a646667ac9ea 142 pid_controller::turn(false);
kolanery 0:cb667de3a336 143 }
kolanery 0:cb667de3a336 144
kolanery 0:cb667de3a336 145 int DriveControl::get_next_direction() {
kolanery 0:cb667de3a336 146 // TODO: Define the direction based on heuristic eval.
kolanery 0:cb667de3a336 147 return 1;
kolanery 0:cb667de3a336 148 }
kolanery 0:cb667de3a336 149
kolanery 0:cb667de3a336 150 int DriveControl::get_next_state(int state) {
kolanery 2:619b02232144 151 // Simply drives the mouse for testing
kolanery 2:619b02232144 152 return DRIVE;
kolanery 0:cb667de3a336 153 }
kolanery 0:cb667de3a336 154
szh66 8:4a32fc9ee939 155 void DriveControl::getEncoder(){
kolanery 14:a646667ac9ea 156 // pc.printf("LEFT Encoder Reading %d\n\r", leftEncoder.getEncoderDistance(1));
kolanery 14:a646667ac9ea 157 // pc.printf("RIGHT Encoder Reading %d\n\r", rightEncoder.getEncoderDistance(0));
kolanery 14:a646667ac9ea 158 pc.printf("FRONT TOP RIGHT IRSensor %f\n\r", rightFrontIR.readIR() * 1000);
kolanery 14:a646667ac9ea 159 pc.printf("FRONT TOP LEFT IRSensor %f\n\r", leftFrontIR.readIR() * 1000);
kolanery 14:a646667ac9ea 160 pc.printf("LEFT Diagonal Sensor %f\n\r", leftDiagonalIR.readIR());
kolanery 14:a646667ac9ea 161 pc.printf("RIGHT Diagonal Sensor %f\n\r", rightDiagonalIR.readIR());
kolanery 14:a646667ac9ea 162 pc.printf("LEFT SIDE Sensor %f\n\r", leftIR.readIR());
kolanery 14:a646667ac9ea 163 pc.printf("RIGHT SIDE Sensor %f\n\r", rightIR.readIR());
szh66 8:4a32fc9ee939 164 }
szh66 8:4a32fc9ee939 165
kolanery 0:cb667de3a336 166 void DriveControl::turn_right() {
kolanery 4:73510c7fa316 167 // TODO: Add PID Control
kolanery 14:a646667ac9ea 168 pid_controller::turn(true);
kolanery 7:7215adbae3da 169 }
kolanery 7:7215adbae3da 170
kolanery 7:7215adbae3da 171 void DriveControl::turn_around() {
kolanery 7:7215adbae3da 172 // TODO: Add PID Control
kolanery 7:7215adbae3da 173 pid_controller::turn(TURN_AROUND);
kolanery 0:cb667de3a336 174 }
kolanery 0:cb667de3a336 175
kolanery 0:cb667de3a336 176 void DriveControl::stop() {
kolanery 2:619b02232144 177 leftMotor->stop();
kolanery 2:619b02232144 178 rightMotor->stop();
kolanery 0:cb667de3a336 179 }
kolanery 0:cb667de3a336 180
kolanery 14:a646667ac9ea 181 void DriveControl::resetEncoders() {
kolanery 14:a646667ac9ea 182 leftEncoder.resetEncoders();
kolanery 14:a646667ac9ea 183 rightEncoder.resetEncoders();
kolanery 14:a646667ac9ea 184 }
kolanery 14:a646667ac9ea 185
kolanery 7:7215adbae3da 186 void DriveControl::drive_forward() {
kolanery 14:a646667ac9ea 187 /*
kolanery 14:a646667ac9ea 188 float COOL_DOWN = 1.0f;
kolanery 14:a646667ac9ea 189 if ((rightFrontIR.readIR() * 1000) > COOL_DOWN) {
kolanery 14:a646667ac9ea 190 pid_controller::cool_down_speed();
kolanery 14:a646667ac9ea 191 }
kolanery 14:a646667ac9ea 192 */
kolanery 14:a646667ac9ea 193
kolanery 7:7215adbae3da 194 pid_controller::navigate();
kolanery 14:a646667ac9ea 195
kolanery 14:a646667ac9ea 196 }
kolanery 14:a646667ac9ea 197
kolanery 14:a646667ac9ea 198 bool DriveControl::should_stop_drive_forward() {
kolanery 14:a646667ac9ea 199 float SHOULD_STOP = 7.0f;
kolanery 14:a646667ac9ea 200 /*
kolanery 14:a646667ac9ea 201 if ((rightFrontIR.readIR() * 1000) > SHOULD_STOP) {
kolanery 14:a646667ac9ea 202 stop();
kolanery 14:a646667ac9ea 203 cool_down_offset = 0.0f;
kolanery 14:a646667ac9ea 204 }*/
kolanery 14:a646667ac9ea 205 return (rightFrontIR.readIR() * 1000) > SHOULD_STOP;
kolanery 14:a646667ac9ea 206 }
kolanery 14:a646667ac9ea 207
kolanery 14:a646667ac9ea 208 bool DriveControl::should_finish_turn_right() {
kolanery 14:a646667ac9ea 209 bool should_turn = (leftEncoder.getEncoderDistance(1) < -16000) && (rightEncoder.getEncoderDistance(0) < -16000);
kolanery 14:a646667ac9ea 210 return should_turn;
kolanery 14:a646667ac9ea 211 }
kolanery 14:a646667ac9ea 212
kolanery 14:a646667ac9ea 213 bool DriveControl::should_finish_turn_left() {
kolanery 14:a646667ac9ea 214 return leftEncoder.getEncoderDistance(1) > 16000
kolanery 14:a646667ac9ea 215 && rightEncoder.getEncoderDistance(0) > 16000;
kolanery 4:73510c7fa316 216 }
kolanery 4:73510c7fa316 217
kolanery 0:cb667de3a336 218
kolanery 7:7215adbae3da 219 // TODO: Test top right ir sensor
kolanery 0:cb667de3a336 220 bool DriveControl::has_front_wall() {
kolanery 14:a646667ac9ea 221 //return rightFrontIR > FRONT_SENSOR_THRESHOLD;
kolanery 14:a646667ac9ea 222 return (rightFrontIR.readIR() * 1000) > 7.0f;
kolanery 0:cb667de3a336 223 }
kolanery 0:cb667de3a336 224
kolanery 0:cb667de3a336 225 bool DriveControl::has_left_wall() {
kolanery 14:a646667ac9ea 226 return leftIR.readIR() > SENSOR_LEFT_WALL_THRES;
kolanery 0:cb667de3a336 227 }
kolanery 0:cb667de3a336 228
kolanery 0:cb667de3a336 229 bool DriveControl::has_right_wall() {
kolanery 14:a646667ac9ea 230 return rightIR.readIR() > SENSOR_RIGHT_WALL_THRES;
kolanery 0:cb667de3a336 231 }