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@12:6f48afe41cd9, 2017-05-17 (annotated)

Committer:
ryan_whr
Date:
Wed May 17 00:18:14 2017 +0000
Revision:
12:6f48afe41cd9
Parent:
8:4a32fc9ee939
Child:
13:a1a3418c07f3
add drive one cell

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"
szh66 8:4a32fc9ee939 7 //#include "encoder.h"
kolanery 0:cb667de3a336 8
szh66 6:1bcfda49e146 9 IRSensor leftIR(PA_8, PC_5);
szh66 6:1bcfda49e146 10 IRSensor rightIR(PB_0, PA_4);
kolanery 7:7215adbae3da 11
szh66 8:4a32fc9ee939 12 //Encoder rightEncoder(PA_1, PA_0);
szh66 8:4a32fc9ee939 13 //Encoder leftEncoder(PC_9, PC_8);
szh66 8:4a32fc9ee939 14
kolanery 0:cb667de3a336 15 // Define states for debugging the mouse hardware
szh66 6:1bcfda49e146 16 const int DRIVE = 0, TURN = 2, STOP = 4;
kolanery 7:7215adbae3da 17 const int TURN_LEFT = 0, TURN_RIGHT = 1, TURN_AROUND = 2;
kolanery 7:7215adbae3da 18
kolanery 2:619b02232144 19 LeftMotor * leftMotor;
kolanery 2:619b02232144 20 RightMotor * rightMotor;
kolanery 0:cb667de3a336 21 Cell * curr_cell;
szh66 6:1bcfda49e146 22 Serial pc(PA_9, PA_10);
szh66 6:1bcfda49e146 23
szh66 6:1bcfda49e146 24 float motor_speed;
kolanery 7:7215adbae3da 25 float MOTOR_BASE_SPEED = 15.0f;
kolanery 7:7215adbae3da 26
kolanery 7:7215adbae3da 27 float FRONT_SENSOR_THRESHOLD = 0.90f;
kolanery 7:7215adbae3da 28 const int SENSOR_THRESHOLD = 12;
szh66 6:1bcfda49e146 29 float LEFT_WALL_THRES = 0.46f, RIGHT_WALL_THRES = 0.21f;
szh66 6:1bcfda49e146 30 float SENSOR_ERROR_OFFSET = 0.0f;
szh66 6:1bcfda49e146 31
kolanery 7:7215adbae3da 32
szh66 6:1bcfda49e146 33 float left_speed;
szh66 6:1bcfda49e146 34 float right_speed;
szh66 6:1bcfda49e146 35 float error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f;
szh66 6:1bcfda49e146 36 float total_error = 0.0f;
szh66 6:1bcfda49e146 37 float P = 18.0f, D = 3.0f;
kolanery 4:73510c7fa316 38
kolanery 7:7215adbae3da 39 namespace pid_controller {
kolanery 7:7215adbae3da 40 void navigate() {
kolanery 7:7215adbae3da 41 bool has_left_wall = leftDiagonalIR.readIR() > LEFT_WALL_THRES;
kolanery 7:7215adbae3da 42 bool has_right_wall = rightDiagonalIR.readIR() > RIGHT_WALL_THRES;
kolanery 7:7215adbae3da 43
kolanery 7:7215adbae3da 44 if (has_left_wall && has_right_wall) {
kolanery 7:7215adbae3da 45 if (rightDiagonalIR - RIGHT_WALL_THRES < 0) {
kolanery 7:7215adbae3da 46 error_p = rightDiagonalIR - RIGHT_WALL_THRES;
kolanery 7:7215adbae3da 47 }
kolanery 7:7215adbae3da 48 else if (leftDiagonalIR - LEFT_WALL_THRES < 0) {
kolanery 7:7215adbae3da 49 error_p = leftDiagonalIR - LEFT_WALL_THRES;
kolanery 7:7215adbae3da 50 }
kolanery 7:7215adbae3da 51 else{
kolanery 7:7215adbae3da 52 error_p = rightDiagonalIR - leftDiagonalIR;
kolanery 7:7215adbae3da 53 }
kolanery 7:7215adbae3da 54 error_d = error_p - old_error_p;
kolanery 7:7215adbae3da 55 }
kolanery 7:7215adbae3da 56 else if (has_left_wall) {
kolanery 7:7215adbae3da 57 error_p = 2 * (LEFT_WALL_THRES - leftDiagonalIR.readIR());
kolanery 7:7215adbae3da 58 error_d = error_p - old_error_p;
kolanery 7:7215adbae3da 59 }
kolanery 7:7215adbae3da 60 else if (has_right_wall) {
kolanery 7:7215adbae3da 61 error_p = 2 * (rightDiagonalIR.readIR() - RIGHT_WALL_THRES);
kolanery 7:7215adbae3da 62 error_d = error_p - old_error_p;
kolanery 7:7215adbae3da 63 }
kolanery 7:7215adbae3da 64 else if (!has_left_wall && !has_right_wall) {
kolanery 7:7215adbae3da 65 error_p = 0;
kolanery 7:7215adbae3da 66 error_d = 0;
kolanery 7:7215adbae3da 67 }
kolanery 7:7215adbae3da 68 total_error = P * error_p + D * (error_d - old_error_d);
kolanery 7:7215adbae3da 69 old_error_p = error_p;
kolanery 7:7215adbae3da 70 old_error_d = error_d;
kolanery 7:7215adbae3da 71
kolanery 7:7215adbae3da 72
kolanery 7:7215adbae3da 73 if(total_error < 7.5f){
kolanery 7:7215adbae3da 74 leftMotor -> speed(MOTOR_BASE_SPEED - total_error);
kolanery 7:7215adbae3da 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 7:7215adbae3da 80 }
ryan_whr 12:6f48afe41cd9 81
ryan_whr 12:6f48afe41cd9 82 one_cell();
ryan_whr 12:6f48afe41cd9 83 }
ryan_whr 12:6f48afe41cd9 84
ryan_whr 12:6f48afe41cd9 85 void one_cell(){
ryan_whr 12:6f48afe41cd9 86 leftEncoder.reset();
ryan_whr 12:6f48afe41cd9 87 rightEncoder.reset();
ryan_whr 12:6f48afe41cd9 88 while(leftEncoder.getEncoderDistance(1)<-46000 & leftEncoder.getEncoderDistance(1)<46000){
ryan_whr 12:6f48afe41cd9 89 //Do nothing
ryan_whr 12:6f48afe41cd9 90 }
kolanery 7:7215adbae3da 91 }
kolanery 7:7215adbae3da 92
kolanery 7:7215adbae3da 93 // todo
kolanery 7:7215adbae3da 94 void turn (int dir) {
kolanery 7:7215adbae3da 95 // https://github.com/austinxiao-ucsd/Falcon-MicroMouse/blob/master/Micromouse_test/drive_control.h
kolanery 7:7215adbae3da 96 if (TURN_LEFT) { // Flip motor
kolanery 7:7215adbae3da 97 }
kolanery 7:7215adbae3da 98 else { // TODO: ...
kolanery 7:7215adbae3da 99 }
kolanery 7:7215adbae3da 100 }
kolanery 7:7215adbae3da 101 }
kolanery 7:7215adbae3da 102
kolanery 0:cb667de3a336 103 // Currently only have the x, y position fields for
kolanery 0:cb667de3a336 104 // each cell.
kolanery 0:cb667de3a336 105 DriveControl::DriveControl (int start_x, int start_y) {
kolanery 0:cb667de3a336 106 curr_cell = new Cell (start_x, start_y);
kolanery 2:619b02232144 107 leftMotor= new LeftMotor();
kolanery 2:619b02232144 108 rightMotor = new RightMotor();
kolanery 0:cb667de3a336 109 }
kolanery 0:cb667de3a336 110
kolanery 0:cb667de3a336 111 // Defines the next cell to traverse.
kolanery 0:cb667de3a336 112 Cell * next_cell() {
kolanery 0:cb667de3a336 113 return curr_cell;
kolanery 0:cb667de3a336 114 }
kolanery 0:cb667de3a336 115
kolanery 0:cb667de3a336 116 void DriveControl::turn_left() {
kolanery 0:cb667de3a336 117 // TODO: Add PID Control
kolanery 7:7215adbae3da 118 pid_controller::turn(TURN_LEFT);
kolanery 0:cb667de3a336 119 }
kolanery 0:cb667de3a336 120
kolanery 0:cb667de3a336 121 int DriveControl::get_next_direction() {
kolanery 0:cb667de3a336 122 // TODO: Define the direction based on heuristic eval.
kolanery 0:cb667de3a336 123 return 1;
kolanery 0:cb667de3a336 124 }
kolanery 0:cb667de3a336 125
kolanery 0:cb667de3a336 126 int DriveControl::get_next_state(int state) {
kolanery 2:619b02232144 127 // Simply drives the mouse for testing
kolanery 2:619b02232144 128 return DRIVE;
kolanery 0:cb667de3a336 129 }
kolanery 0:cb667de3a336 130
szh66 8:4a32fc9ee939 131 void DriveControl::getEncoder(){
szh66 8:4a32fc9ee939 132 pc.printf("LeftEncoder Reading %d", leftEncoder.getEncoderDistance(1));
szh66 8:4a32fc9ee939 133 pc.printf("RightEncoder Reading %d\n\r", rightEncoder.getEncoderDistance(0));
szh66 8:4a32fc9ee939 134 }
szh66 8:4a32fc9ee939 135
kolanery 0:cb667de3a336 136 void DriveControl::turn_right() {
kolanery 4:73510c7fa316 137 // TODO: Add PID Control
kolanery 7:7215adbae3da 138 pid_controller::turn(TURN_RIGHT);
kolanery 7:7215adbae3da 139 }
kolanery 7:7215adbae3da 140
kolanery 7:7215adbae3da 141 void DriveControl::turn_around() {
kolanery 7:7215adbae3da 142 // TODO: Add PID Control
kolanery 7:7215adbae3da 143 pid_controller::turn(TURN_AROUND);
kolanery 0:cb667de3a336 144 }
kolanery 0:cb667de3a336 145
kolanery 0:cb667de3a336 146 void DriveControl::stop() {
kolanery 2:619b02232144 147 leftMotor->stop();
kolanery 2:619b02232144 148 rightMotor->stop();
kolanery 0:cb667de3a336 149 }
kolanery 0:cb667de3a336 150
kolanery 7:7215adbae3da 151 void DriveControl::drive_forward() {
kolanery 7:7215adbae3da 152 pid_controller::navigate();
kolanery 4:73510c7fa316 153 }
kolanery 4:73510c7fa316 154
kolanery 0:cb667de3a336 155
kolanery 7:7215adbae3da 156 // TODO: Test top right ir sensor
kolanery 0:cb667de3a336 157 bool DriveControl::has_front_wall() {
kolanery 7:7215adbae3da 158 return rightFrontIR > FRONT_SENSOR_THRESHOLD;
kolanery 0:cb667de3a336 159 }
kolanery 0:cb667de3a336 160
kolanery 0:cb667de3a336 161 bool DriveControl::has_left_wall() {
kolanery 0:cb667de3a336 162 return leftIR < SENSOR_THRESHOLD;
kolanery 0:cb667de3a336 163 }
kolanery 0:cb667de3a336 164
kolanery 0:cb667de3a336 165 bool DriveControl::has_right_wall() {
kolanery 0:cb667de3a336 166 return rightIR < SENSOR_THRESHOLD;
kolanery 0:cb667de3a336 167 }