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@18:85196734207e, 2017-05-20 (annotated)

Committer:
kolanery
Date:
Sat May 20 22:37:18 2017 +0000
Revision:
18:85196734207e
Parent:
17:043ed1d0196f 
restructure project

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