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@31:f7a8a9b82bc1, 2017-05-28 (annotated)

Committer:
kolanery
Date:
Sun May 28 09:54:40 2017 +0000
Revision:
31:f7a8a9b82bc1
Parent:
30:daf286ac049f 
Update

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 // Sensor offsets
kolanery 17:043ed1d0196f 18 //float FRONT_SENSOR_THRES = 7.0f, SENSOR_ERROR_OFFSET = 0.0f;
ryan_whr 22:1a02d66218e4 19 float FRONT_SENSOR_THRES = 2.2f, SENSOR_ERROR_OFFSET = 0.0f;
kolanery 30:daf286ac049f 20 //float LEFT_WALL_THRES = 0.336f, RIGHT_WALL_THRES = 0.163f;
kolanery 30:daf286ac049f 21 float LEFT_WALL_THRES = 0.336f, RIGHT_WALL_THRES = 0.163f;
kolanery 30:daf286ac049f 22 float RIGHT_SIDE_WALL_THRES = 0.20f, LEFT_SIDE_WALL_THRES = 0.20f;
kolanery 30:daf286ac049f 23 //float RIGHT_SIDE_WALL_THRES = 0.28f, LEFT_SIDE_WALL_THRES = 0.38f;
kolanery 13:a1a3418c07f3 24
kolanery 13:a1a3418c07f3 25 // Motor speed offsets
kolanery 13:a1a3418c07f3 26 float left_speed, right_speed, motor_speed;
kolanery 30:daf286ac049f 27 float MOTOR_BASE_SPEED = 20.0f; //12.0f;
kolanery 14:a646667ac9ea 28 float cool_down_offset = 0.0f;
kolanery 31:f7a8a9b82bc1 29 //
kolanery 31:f7a8a9b82bc1 30 // bool right_front_wall = (rightFrontIR.readIR() * 1000) > 355.0f;//350.39f;
kolanery 31:f7a8a9b82bc1 31 // bool left_front_wall = (leftFrontIR.readIR() * 1000) > 315.0f;//310.25f;
kolanery 31:f7a8a9b82bc1 32
kolanery 31:f7a8a9b82bc1 33 float RIGHT_FRONT_THRES = 355.0f;
kolanery 31:f7a8a9b82bc1 34 float LEFT_FRONT_THRES = 315.0f;
kolanery 7:7215adbae3da 35
kolanery 28:600932142201 36 bool enter_right_wall_pid_debug = false;
kolanery 28:600932142201 37
kolanery 16:c26d8e007df5 38 // Encoder offsets
kolanery 30:daf286ac049f 39 int ENCODER_TURN_UNIT = 16500;
kolanery 16:c26d8e007df5 40
kolanery 13:a1a3418c07f3 41 namespace pid_controller {
kolanery 13:a1a3418c07f3 42 // PID Constants
kolanery 13:a1a3418c07f3 43 float error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f;
kolanery 13:a1a3418c07f3 44 float total_error = 0.0f;
kolanery 30:daf286ac049f 45 float P = 18.0f, D = 20.0f;
kolanery 13:a1a3418c07f3 46
kolanery 7:7215adbae3da 47 void navigate() {
kolanery 7:7215adbae3da 48 bool has_left_wall = leftDiagonalIR.readIR() > LEFT_WALL_THRES;
kolanery 7:7215adbae3da 49 bool has_right_wall = rightDiagonalIR.readIR() > RIGHT_WALL_THRES;
kolanery 7:7215adbae3da 50
kolanery 7:7215adbae3da 51 if (has_left_wall && has_right_wall) {
kolanery 28:600932142201 52 enter_right_wall_pid_debug = false;
kolanery 7:7215adbae3da 53 if (rightDiagonalIR - RIGHT_WALL_THRES < 0) {
kolanery 7:7215adbae3da 54 error_p = rightDiagonalIR - RIGHT_WALL_THRES;
kolanery 7:7215adbae3da 55 }
kolanery 7:7215adbae3da 56 else if (leftDiagonalIR - LEFT_WALL_THRES < 0) {
kolanery 30:daf286ac049f 57 error_p = LEFT_WALL_THRES - leftDiagonalIR;
kolanery 7:7215adbae3da 58 }
kolanery 7:7215adbae3da 59 else{
kolanery 7:7215adbae3da 60 error_p = rightDiagonalIR - leftDiagonalIR;
kolanery 30:daf286ac049f 61 enter_right_wall_pid_debug = true;
kolanery 7:7215adbae3da 62 }
kolanery 30:daf286ac049f 63 error_d = error_p - old_error_p;
kolanery 30:daf286ac049f 64
kolanery 7:7215adbae3da 65 }
kolanery 28:600932142201 66 // float RIGHT_SIDE_WALL_THRES = 0.59f, LEFT_SIDE_WALL_THRES = 0.38f;
kolanery 7:7215adbae3da 67 else if (has_left_wall) {
kolanery 28:600932142201 68 // error_p = 4 * (LEFT_WALL_THRES - 0.50 * leftDiagonalIR.readIR());
kolanery 30:daf286ac049f 69 error_p = LEFT_WALL_THRES - leftDiagonalIR.readIR();
kolanery 28:600932142201 70 //error_p = 2 * (LEFT_SIDE_WALL_THRES - leftIR.readIR());
kolanery 7:7215adbae3da 71 error_d = error_p - old_error_p;
kolanery 30:daf286ac049f 72 enter_right_wall_pid_debug = false;
kolanery 7:7215adbae3da 73 }
kolanery 7:7215adbae3da 74 else if (has_right_wall) {
kolanery 30:daf286ac049f 75 error_p = rightDiagonalIR.readIR() - RIGHT_WALL_THRES;
kolanery 30:daf286ac049f 76 //error_p = 2 * (RIGHT_SIDE_WALL_THRES - rightIR.readIR();
kolanery 7:7215adbae3da 77 error_d = error_p - old_error_p;
kolanery 28:600932142201 78 enter_right_wall_pid_debug = false;
kolanery 7:7215adbae3da 79 }
kolanery 7:7215adbae3da 80 else if (!has_left_wall && !has_right_wall) {
kolanery 7:7215adbae3da 81 error_p = 0;
kolanery 7:7215adbae3da 82 error_d = 0;
kolanery 28:600932142201 83 enter_right_wall_pid_debug = false;
kolanery 7:7215adbae3da 84 }
kolanery 7:7215adbae3da 85 total_error = P * error_p + D * (error_d - old_error_d);
kolanery 7:7215adbae3da 86 old_error_p = error_p;
kolanery 7:7215adbae3da 87 old_error_d = error_d;
kolanery 7:7215adbae3da 88
kolanery 14:a646667ac9ea 89 //MOTOR_BASE_SPEED -= cool_down_offset;
kolanery 16:c26d8e007df5 90 if(total_error < 7.5f){
kolanery 7:7215adbae3da 91 leftMotor -> speed(MOTOR_BASE_SPEED - total_error);
kolanery 7:7215adbae3da 92 rightMotor -> speed(MOTOR_BASE_SPEED + total_error);
kolanery 7:7215adbae3da 93 }
kolanery 7:7215adbae3da 94 else{
kolanery 7:7215adbae3da 95 leftMotor->speed(MOTOR_BASE_SPEED);
kolanery 7:7215adbae3da 96 rightMotor->speed(MOTOR_BASE_SPEED);
kolanery 14:a646667ac9ea 97 }
kolanery 14:a646667ac9ea 98
kolanery 14:a646667ac9ea 99
kolanery 14:a646667ac9ea 100 }
kolanery 14:a646667ac9ea 101
kolanery 16:c26d8e007df5 102 void clear_pid(){
kolanery 16:c26d8e007df5 103 error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f;
kolanery 16:c26d8e007df5 104 total_error = 0.0f;
kolanery 16:c26d8e007df5 105 }
kolanery 16:c26d8e007df5 106
kolanery 14:a646667ac9ea 107 void cool_down_speed() {
kolanery 14:a646667ac9ea 108 if (cool_down_offset < 5.0f) {
kolanery 14:a646667ac9ea 109 cool_down_offset += 2.0f;
kolanery 7:7215adbae3da 110 }
ryan_whr 12:6f48afe41cd9 111 }
ryan_whr 12:6f48afe41cd9 112
kolanery 13:a1a3418c07f3 113 // should use this method as the exit condition
kolanery 13:a1a3418c07f3 114 // in the pid_controller::navigate() method
kolanery 13:a1a3418c07f3 115 // resets the pid, encoders, etc.
kolanery 13:a1a3418c07f3 116 void one_cell_traversed() {
kolanery 13:a1a3418c07f3 117 leftEncoder.reset();
kolanery 13:a1a3418c07f3 118 rightEncoder.reset();
kolanery 13:a1a3418c07f3 119 old_error_p = old_error_d = total_error = 0.0f;
kolanery 13:a1a3418c07f3 120 }
kolanery 13:a1a3418c07f3 121
kolanery 13:a1a3418c07f3 122 // TODO
kolanery 14:a646667ac9ea 123 void turn (bool turn_right) {
kolanery 16:c26d8e007df5 124 float MOTOR_TURN_SPEED = 14.0f;
kolanery 15:151e59899221 125
kolanery 15:151e59899221 126 if (turn_right) {
kolanery 14:a646667ac9ea 127 leftMotor -> speed(MOTOR_TURN_SPEED);
kolanery 15:151e59899221 128 rightMotor -> speed(-MOTOR_TURN_SPEED);
kolanery 7:7215adbae3da 129 }
kolanery 14:a646667ac9ea 130 else {
kolanery 15:151e59899221 131 leftMotor -> speed(-MOTOR_TURN_SPEED);
kolanery 14:a646667ac9ea 132 rightMotor -> speed(MOTOR_TURN_SPEED);
kolanery 7:7215adbae3da 133 }
kolanery 7:7215adbae3da 134 }
kolanery 7:7215adbae3da 135 }
kolanery 28:600932142201 136
kolanery 28:600932142201 137 bool DriveControl::right_wall_pid_debug() {
kolanery 28:600932142201 138 return enter_right_wall_pid_debug;
kolanery 28:600932142201 139 }
kolanery 0:cb667de3a336 140 // Currently only have the x, y position fields for
kolanery 0:cb667de3a336 141 // each cell.
kolanery 0:cb667de3a336 142 DriveControl::DriveControl (int start_x, int start_y) {
kolanery 0:cb667de3a336 143 curr_cell = new Cell (start_x, start_y);
kolanery 2:619b02232144 144 leftMotor= new LeftMotor();
kolanery 2:619b02232144 145 rightMotor = new RightMotor();
kolanery 0:cb667de3a336 146 }
kolanery 0:cb667de3a336 147
kolanery 0:cb667de3a336 148 // Defines the next cell to traverse.
kolanery 0:cb667de3a336 149 Cell * next_cell() {
kolanery 0:cb667de3a336 150 return curr_cell;
kolanery 0:cb667de3a336 151 }
kolanery 0:cb667de3a336 152
kolanery 0:cb667de3a336 153 int DriveControl::get_next_direction() {
kolanery 0:cb667de3a336 154 // TODO: Define the direction based on heuristic eval.
kolanery 0:cb667de3a336 155 return 1;
kolanery 0:cb667de3a336 156 }
kolanery 0:cb667de3a336 157
kolanery 15:151e59899221 158 void DriveControl::print_serial_ports(){
kolanery 15:151e59899221 159 pc.printf("LEFT Encoder Reading %d\n\r", leftEncoder.getEncoderDistance(1));
kolanery 15:151e59899221 160 pc.printf("RIGHT Encoder Reading %d\n\r", rightEncoder.getEncoderDistance(0));
ryan_whr 22:1a02d66218e4 161 pc.printf("FRONT TOP RIGHT IRSensor %f\n\r", rightFrontIR.readIR() * 1000);
kolanery 28:600932142201 162 pc.printf("FRONT TOP LEFT IRSensor %f\n\r", leftFrontIR.readIR() * 1000);
kolanery 14:a646667ac9ea 163 pc.printf("LEFT Diagonal Sensor %f\n\r", leftDiagonalIR.readIR());
kolanery 14:a646667ac9ea 164 pc.printf("RIGHT Diagonal Sensor %f\n\r", rightDiagonalIR.readIR());
kolanery 14:a646667ac9ea 165 pc.printf("LEFT SIDE Sensor %f\n\r", leftIR.readIR());
kolanery 14:a646667ac9ea 166 pc.printf("RIGHT SIDE Sensor %f\n\r", rightIR.readIR());
kolanery 16:c26d8e007df5 167 }
kolanery 16:c26d8e007df5 168
kolanery 16:c26d8e007df5 169 void DriveControl::turn_left() {
kolanery 16:c26d8e007df5 170 // TODO: Add PID Control
kolanery 16:c26d8e007df5 171 pid_controller::turn(false);
szh66 8:4a32fc9ee939 172 }
szh66 8:4a32fc9ee939 173
kolanery 31:f7a8a9b82bc1 174 void DriveControl::set_wall_follower_sensor_thres(){
kolanery 31:f7a8a9b82bc1 175 //TODO
kolanery 31:f7a8a9b82bc1 176 RIGHT_FRONT_THRES = 300.0f;
kolanery 31:f7a8a9b82bc1 177 LEFT_FRONT_THRES = 250.0f;
kolanery 31:f7a8a9b82bc1 178 }
kolanery 31:f7a8a9b82bc1 179
kolanery 0:cb667de3a336 180 void DriveControl::turn_right() {
kolanery 4:73510c7fa316 181 // TODO: Add PID Control
kolanery 14:a646667ac9ea 182 pid_controller::turn(true);
kolanery 7:7215adbae3da 183 }
kolanery 7:7215adbae3da 184
kolanery 30:daf286ac049f 185 void DriveControl::set_wall_follower_speed() {
kolanery 31:f7a8a9b82bc1 186 MOTOR_BASE_SPEED = 25;
kolanery 30:daf286ac049f 187 }
kolanery 30:daf286ac049f 188
kolanery 7:7215adbae3da 189 void DriveControl::turn_around() {
kolanery 7:7215adbae3da 190 // TODO: Add PID Control
kolanery 15:151e59899221 191 //pid_controller::turn(TURN_AROUND);
kolanery 16:c26d8e007df5 192 pid_controller::turn(true);
kolanery 0:cb667de3a336 193 }
kolanery 0:cb667de3a336 194
kolanery 0:cb667de3a336 195 void DriveControl::stop() {
kolanery 2:619b02232144 196 leftMotor->stop();
kolanery 2:619b02232144 197 rightMotor->stop();
kolanery 0:cb667de3a336 198 }
kolanery 0:cb667de3a336 199
kolanery 14:a646667ac9ea 200 void DriveControl::resetEncoders() {
kolanery 14:a646667ac9ea 201 leftEncoder.resetEncoders();
kolanery 14:a646667ac9ea 202 rightEncoder.resetEncoders();
kolanery 14:a646667ac9ea 203 }
kolanery 14:a646667ac9ea 204
kolanery 7:7215adbae3da 205 void DriveControl::drive_forward() {
kolanery 7:7215adbae3da 206 pid_controller::navigate();
kolanery 14:a646667ac9ea 207
kolanery 14:a646667ac9ea 208 }
kolanery 14:a646667ac9ea 209
kolanery 14:a646667ac9ea 210 bool DriveControl::should_stop_drive_forward() {
kolanery 14:a646667ac9ea 211 float SHOULD_STOP = 7.0f;
kolanery 14:a646667ac9ea 212 return (rightFrontIR.readIR() * 1000) > SHOULD_STOP;
kolanery 14:a646667ac9ea 213 }
kolanery 14:a646667ac9ea 214
kolanery 14:a646667ac9ea 215 bool DriveControl::should_finish_turn_right() {
kolanery 16:c26d8e007df5 216 int max =
kolanery 16:c26d8e007df5 217 (leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0))
kolanery 16:c26d8e007df5 218 ? rightEncoder.getEncoderDistance(0):leftEncoder.getEncoderDistance(1);
kolanery 16:c26d8e007df5 219 return max < - ENCODER_TURN_UNIT;
kolanery 14:a646667ac9ea 220 }
kolanery 14:a646667ac9ea 221
kolanery 14:a646667ac9ea 222 bool DriveControl::should_finish_turn_left() {
kolanery 16:c26d8e007df5 223 int min_two =
kolanery 16:c26d8e007df5 224 (leftEncoder.getEncoderDistance(1) > rightEncoder.getEncoderDistance(0))
kolanery 16:c26d8e007df5 225 ? rightEncoder.getEncoderDistance(0):leftEncoder.getEncoderDistance(1);
kolanery 16:c26d8e007df5 226 return min_two > ENCODER_TURN_UNIT;
kolanery 16:c26d8e007df5 227 }
kolanery 16:c26d8e007df5 228
kolanery 16:c26d8e007df5 229 bool DriveControl::should_finish_drive_forward() {
kolanery 16:c26d8e007df5 230 int max_two =
kolanery 16:c26d8e007df5 231 (- leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0))
kolanery 16:c26d8e007df5 232 ? rightEncoder.getEncoderDistance(0):- leftEncoder.getEncoderDistance(1);
kolanery 30:daf286ac049f 233 return max_two > 36000;
kolanery 4:73510c7fa316 234 }
kolanery 4:73510c7fa316 235
kolanery 0:cb667de3a336 236 bool DriveControl::has_front_wall() {
kolanery 31:f7a8a9b82bc1 237 // float RIGHT_FRONT_THRES = 355.0f;
kolanery 31:f7a8a9b82bc1 238 //float LEFT_FRONT_THRES = 315.0f;
kolanery 31:f7a8a9b82bc1 239 bool right_front_wall = (rightFrontIR.readIR() * 1000) > RIGHT_FRONT_THRES;//350.39f;
kolanery 31:f7a8a9b82bc1 240 bool left_front_wall = (leftFrontIR.readIR() * 1000) > LEFT_FRONT_THRES;//310.25f;
szh66 27:b980fce784ea 241 return right_front_wall || left_front_wall;
kolanery 0:cb667de3a336 242 }
kolanery 0:cb667de3a336 243
kolanery 0:cb667de3a336 244 bool DriveControl::has_left_wall() {
kolanery 15:151e59899221 245 return leftIR.readIR() > LEFT_SIDE_WALL_THRES;
kolanery 0:cb667de3a336 246 }
kolanery 0:cb667de3a336 247
kolanery 0:cb667de3a336 248 bool DriveControl::has_right_wall() {
kolanery 15:151e59899221 249 return rightIR.readIR() > RIGHT_SIDE_WALL_THRES;
kolanery 0:cb667de3a336 250 }
kolanery 16:c26d8e007df5 251
kolanery 16:c26d8e007df5 252 void DriveControl::clear_pid() {
kolanery 16:c26d8e007df5 253 pid_controller::clear_pid();
kolanery 16:c26d8e007df5 254 }