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@27:b980fce784ea, 2017-05-26 (annotated)
- Committer:
- szh66
- Date:
- Fri May 26 05:45:02 2017 +0000
- Revision:
- 27:b980fce784ea
- Parent:
- 26:191ec0e78774
- Child:
- 28:600932142201
Refactor FloodFill 1045 May 25
Who changed what in which revision?
User | Revision | Line number | New 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; |
szh66 | 27:b980fce784ea | 20 | float LEFT_WALL_THRES = 0.3643f, RIGHT_WALL_THRES = 0.1932f; |
szh66 | 27:b980fce784ea | 21 | float RIGHT_SIDE_WALL_THRES = 0.32f, LEFT_SIDE_WALL_THRES = 0.28f; |
kolanery | 13:a1a3418c07f3 | 22 | |
kolanery | 13:a1a3418c07f3 | 23 | // Motor speed offsets |
kolanery | 13:a1a3418c07f3 | 24 | float left_speed, right_speed, motor_speed; |
ryan_whr | 22:1a02d66218e4 | 25 | float MOTOR_BASE_SPEED = 12.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 | 7:7215adbae3da | 73 | } |
kolanery | 7:7215adbae3da | 74 | else{ |
kolanery | 7:7215adbae3da | 75 | leftMotor->speed(MOTOR_BASE_SPEED); |
kolanery | 7:7215adbae3da | 76 | rightMotor->speed(MOTOR_BASE_SPEED); |
kolanery | 14:a646667ac9ea | 77 | } |
kolanery | 14:a646667ac9ea | 78 | |
kolanery | 14:a646667ac9ea | 79 | |
kolanery | 14:a646667ac9ea | 80 | } |
kolanery | 14:a646667ac9ea | 81 | |
kolanery | 16:c26d8e007df5 | 82 | void clear_pid(){ |
kolanery | 16:c26d8e007df5 | 83 | error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f; |
kolanery | 16:c26d8e007df5 | 84 | total_error = 0.0f; |
kolanery | 16:c26d8e007df5 | 85 | } |
kolanery | 16:c26d8e007df5 | 86 | |
kolanery | 14:a646667ac9ea | 87 | void cool_down_speed() { |
kolanery | 14:a646667ac9ea | 88 | if (cool_down_offset < 5.0f) { |
kolanery | 14:a646667ac9ea | 89 | cool_down_offset += 2.0f; |
kolanery | 7:7215adbae3da | 90 | } |
ryan_whr | 12:6f48afe41cd9 | 91 | } |
ryan_whr | 12:6f48afe41cd9 | 92 | |
kolanery | 13:a1a3418c07f3 | 93 | void one_cell_turned(){ |
ryan_whr | 12:6f48afe41cd9 | 94 | leftEncoder.reset(); |
ryan_whr | 12:6f48afe41cd9 | 95 | rightEncoder.reset(); |
ryan_whr | 25:7155cb993870 | 96 | while(leftEncoder.getEncoderDistance(1)<-45000 & leftEncoder.getEncoderDistance(1)<45000){ |
ryan_whr | 12:6f48afe41cd9 | 97 | //Do nothing |
ryan_whr | 12:6f48afe41cd9 | 98 | } |
kolanery | 7:7215adbae3da | 99 | } |
kolanery | 7:7215adbae3da | 100 | |
kolanery | 13:a1a3418c07f3 | 101 | // should use this method as the exit condition |
kolanery | 13:a1a3418c07f3 | 102 | // in the pid_controller::navigate() method |
kolanery | 13:a1a3418c07f3 | 103 | // resets the pid, encoders, etc. |
kolanery | 13:a1a3418c07f3 | 104 | void one_cell_traversed() { |
kolanery | 13:a1a3418c07f3 | 105 | leftEncoder.reset(); |
kolanery | 13:a1a3418c07f3 | 106 | rightEncoder.reset(); |
kolanery | 13:a1a3418c07f3 | 107 | old_error_p = old_error_d = total_error = 0.0f; |
kolanery | 13:a1a3418c07f3 | 108 | } |
kolanery | 13:a1a3418c07f3 | 109 | |
kolanery | 13:a1a3418c07f3 | 110 | // TODO |
kolanery | 14:a646667ac9ea | 111 | void turn (bool turn_right) { |
kolanery | 16:c26d8e007df5 | 112 | float MOTOR_TURN_SPEED = 14.0f; |
kolanery | 15:151e59899221 | 113 | |
kolanery | 15:151e59899221 | 114 | if (turn_right) { |
kolanery | 14:a646667ac9ea | 115 | leftMotor -> speed(MOTOR_TURN_SPEED); |
kolanery | 15:151e59899221 | 116 | rightMotor -> speed(-MOTOR_TURN_SPEED); |
kolanery | 7:7215adbae3da | 117 | } |
kolanery | 14:a646667ac9ea | 118 | else { |
kolanery | 15:151e59899221 | 119 | leftMotor -> speed(-MOTOR_TURN_SPEED); |
kolanery | 14:a646667ac9ea | 120 | rightMotor -> speed(MOTOR_TURN_SPEED); |
kolanery | 7:7215adbae3da | 121 | } |
kolanery | 7:7215adbae3da | 122 | } |
kolanery | 7:7215adbae3da | 123 | } |
kolanery | 0:cb667de3a336 | 124 | // Currently only have the x, y position fields for |
kolanery | 0:cb667de3a336 | 125 | // each cell. |
kolanery | 0:cb667de3a336 | 126 | DriveControl::DriveControl (int start_x, int start_y) { |
kolanery | 0:cb667de3a336 | 127 | curr_cell = new Cell (start_x, start_y); |
kolanery | 2:619b02232144 | 128 | leftMotor= new LeftMotor(); |
kolanery | 2:619b02232144 | 129 | rightMotor = new RightMotor(); |
kolanery | 0:cb667de3a336 | 130 | } |
kolanery | 0:cb667de3a336 | 131 | |
kolanery | 0:cb667de3a336 | 132 | // Defines the next cell to traverse. |
kolanery | 0:cb667de3a336 | 133 | Cell * next_cell() { |
kolanery | 0:cb667de3a336 | 134 | return curr_cell; |
kolanery | 0:cb667de3a336 | 135 | } |
kolanery | 0:cb667de3a336 | 136 | |
kolanery | 0:cb667de3a336 | 137 | int DriveControl::get_next_direction() { |
kolanery | 0:cb667de3a336 | 138 | // TODO: Define the direction based on heuristic eval. |
kolanery | 0:cb667de3a336 | 139 | return 1; |
kolanery | 0:cb667de3a336 | 140 | } |
kolanery | 0:cb667de3a336 | 141 | |
kolanery | 15:151e59899221 | 142 | void DriveControl::print_serial_ports(){ |
kolanery | 15:151e59899221 | 143 | pc.printf("LEFT Encoder Reading %d\n\r", leftEncoder.getEncoderDistance(1)); |
kolanery | 15:151e59899221 | 144 | pc.printf("RIGHT Encoder Reading %d\n\r", rightEncoder.getEncoderDistance(0)); |
ryan_whr | 22:1a02d66218e4 | 145 | pc.printf("FRONT TOP RIGHT IRSensor %f\n\r", rightFrontIR.readIR() * 1000); |
kolanery | 15:151e59899221 | 146 | pc.printf("FRONT TOP LEFT IRSensor %f\n\r", leftFrontIR.readIR() * 10); |
kolanery | 14:a646667ac9ea | 147 | pc.printf("LEFT Diagonal Sensor %f\n\r", leftDiagonalIR.readIR()); |
kolanery | 14:a646667ac9ea | 148 | pc.printf("RIGHT Diagonal Sensor %f\n\r", rightDiagonalIR.readIR()); |
kolanery | 14:a646667ac9ea | 149 | pc.printf("LEFT SIDE Sensor %f\n\r", leftIR.readIR()); |
kolanery | 14:a646667ac9ea | 150 | pc.printf("RIGHT SIDE Sensor %f\n\r", rightIR.readIR()); |
kolanery | 16:c26d8e007df5 | 151 | } |
kolanery | 16:c26d8e007df5 | 152 | |
kolanery | 16:c26d8e007df5 | 153 | void DriveControl::turn_left() { |
kolanery | 16:c26d8e007df5 | 154 | // TODO: Add PID Control |
kolanery | 16:c26d8e007df5 | 155 | pid_controller::turn(false); |
szh66 | 8:4a32fc9ee939 | 156 | } |
szh66 | 8:4a32fc9ee939 | 157 | |
kolanery | 0:cb667de3a336 | 158 | void DriveControl::turn_right() { |
kolanery | 4:73510c7fa316 | 159 | // TODO: Add PID Control |
kolanery | 14:a646667ac9ea | 160 | pid_controller::turn(true); |
kolanery | 7:7215adbae3da | 161 | } |
kolanery | 7:7215adbae3da | 162 | |
kolanery | 7:7215adbae3da | 163 | void DriveControl::turn_around() { |
kolanery | 7:7215adbae3da | 164 | // TODO: Add PID Control |
kolanery | 15:151e59899221 | 165 | //pid_controller::turn(TURN_AROUND); |
kolanery | 16:c26d8e007df5 | 166 | pid_controller::turn(true); |
kolanery | 0:cb667de3a336 | 167 | } |
kolanery | 0:cb667de3a336 | 168 | |
kolanery | 0:cb667de3a336 | 169 | void DriveControl::stop() { |
kolanery | 2:619b02232144 | 170 | leftMotor->stop(); |
kolanery | 2:619b02232144 | 171 | rightMotor->stop(); |
kolanery | 0:cb667de3a336 | 172 | } |
kolanery | 0:cb667de3a336 | 173 | |
kolanery | 14:a646667ac9ea | 174 | void DriveControl::resetEncoders() { |
kolanery | 14:a646667ac9ea | 175 | leftEncoder.resetEncoders(); |
kolanery | 14:a646667ac9ea | 176 | rightEncoder.resetEncoders(); |
kolanery | 14:a646667ac9ea | 177 | } |
kolanery | 14:a646667ac9ea | 178 | |
kolanery | 7:7215adbae3da | 179 | void DriveControl::drive_forward() { |
kolanery | 7:7215adbae3da | 180 | pid_controller::navigate(); |
kolanery | 14:a646667ac9ea | 181 | |
kolanery | 14:a646667ac9ea | 182 | } |
kolanery | 14:a646667ac9ea | 183 | |
kolanery | 14:a646667ac9ea | 184 | bool DriveControl::should_stop_drive_forward() { |
kolanery | 14:a646667ac9ea | 185 | float SHOULD_STOP = 7.0f; |
kolanery | 14:a646667ac9ea | 186 | return (rightFrontIR.readIR() * 1000) > SHOULD_STOP; |
kolanery | 14:a646667ac9ea | 187 | } |
kolanery | 14:a646667ac9ea | 188 | |
kolanery | 14:a646667ac9ea | 189 | bool DriveControl::should_finish_turn_right() { |
kolanery | 16:c26d8e007df5 | 190 | int max = |
kolanery | 16:c26d8e007df5 | 191 | (leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0)) |
kolanery | 16:c26d8e007df5 | 192 | ? rightEncoder.getEncoderDistance(0):leftEncoder.getEncoderDistance(1); |
kolanery | 16:c26d8e007df5 | 193 | return max < - ENCODER_TURN_UNIT; |
kolanery | 14:a646667ac9ea | 194 | } |
kolanery | 14:a646667ac9ea | 195 | |
kolanery | 14:a646667ac9ea | 196 | bool DriveControl::should_finish_turn_left() { |
kolanery | 16:c26d8e007df5 | 197 | int min_two = |
kolanery | 16:c26d8e007df5 | 198 | (leftEncoder.getEncoderDistance(1) > rightEncoder.getEncoderDistance(0)) |
kolanery | 16:c26d8e007df5 | 199 | ? rightEncoder.getEncoderDistance(0):leftEncoder.getEncoderDistance(1); |
kolanery | 16:c26d8e007df5 | 200 | return min_two > ENCODER_TURN_UNIT; |
kolanery | 16:c26d8e007df5 | 201 | } |
kolanery | 16:c26d8e007df5 | 202 | |
kolanery | 16:c26d8e007df5 | 203 | bool DriveControl::should_finish_drive_forward() { |
kolanery | 16:c26d8e007df5 | 204 | int max_two = |
kolanery | 16:c26d8e007df5 | 205 | (- leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0)) |
kolanery | 16:c26d8e007df5 | 206 | ? rightEncoder.getEncoderDistance(0):- leftEncoder.getEncoderDistance(1); |
ryan_whr | 25:7155cb993870 | 207 | return max_two > 45000; |
kolanery | 4:73510c7fa316 | 208 | } |
kolanery | 4:73510c7fa316 | 209 | |
kolanery | 0:cb667de3a336 | 210 | bool DriveControl::has_front_wall() { |
szh66 | 27:b980fce784ea | 211 | bool right_front_wall = (rightFrontIR.readIR() * 500) > FRONT_SENSOR_THRES; |
kolanery | 15:151e59899221 | 212 | bool left_front_wall = (leftFrontIR.readIR() * 10) > FRONT_SENSOR_THRES; |
szh66 | 27:b980fce784ea | 213 | return right_front_wall || left_front_wall; |
kolanery | 0:cb667de3a336 | 214 | } |
kolanery | 0:cb667de3a336 | 215 | |
kolanery | 0:cb667de3a336 | 216 | bool DriveControl::has_left_wall() { |
kolanery | 15:151e59899221 | 217 | return leftIR.readIR() > LEFT_SIDE_WALL_THRES; |
kolanery | 0:cb667de3a336 | 218 | } |
kolanery | 0:cb667de3a336 | 219 | |
kolanery | 0:cb667de3a336 | 220 | bool DriveControl::has_right_wall() { |
kolanery | 15:151e59899221 | 221 | return rightIR.readIR() > RIGHT_SIDE_WALL_THRES; |
kolanery | 0:cb667de3a336 | 222 | } |
kolanery | 16:c26d8e007df5 | 223 | |
kolanery | 16:c26d8e007df5 | 224 | void DriveControl::clear_pid() { |
kolanery | 16:c26d8e007df5 | 225 | pid_controller::clear_pid(); |
kolanery | 16:c26d8e007df5 | 226 | } |