Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the human driver (RF controller) program for the Robotics Cat and Mouse program.
Dependencies: ADXL345_I2C_NEST HMC6352 IMUfilter ITG3200_NEST USBHost mbed-rtos mbed
Fork of Project by
AIControl.h@6:3fb9f96765f6, 2014-04-30 (annotated)
- Committer:
- Strikewolf
- Date:
- Wed Apr 30 12:23:04 2014 +0000
- Revision:
- 6:3fb9f96765f6
- Parent:
- 5:210cd333f770
Final product before demo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Strikewolf | 5:210cd333f770 | 1 | //******************************************* |
Strikewolf | 5:210cd333f770 | 2 | //* Robot motor control and drive functions * |
Strikewolf | 5:210cd333f770 | 3 | //******************************************* |
Strikewolf | 5:210cd333f770 | 4 | |
Strikewolf | 5:210cd333f770 | 5 | #define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms |
Strikewolf | 5:210cd333f770 | 6 | |
Strikewolf | 5:210cd333f770 | 7 | //Radio Calibration Signals - Need to implement calibration routine... |
Strikewolf | 5:210cd333f770 | 8 | #define CHAN3_MAX 2060 //Throttle Hi |
Strikewolf | 5:210cd333f770 | 9 | #define CHAN3_MIN 1150 ` //Throttle Lo |
Strikewolf | 5:210cd333f770 | 10 | #define CHAN2_MAX 1260 //Elevator Hi |
Strikewolf | 5:210cd333f770 | 11 | #define CHAN2_MIN 2290 //Elevator Lo |
Strikewolf | 5:210cd333f770 | 12 | #define CHAN1_MAX 2160 //Rudder Hi |
Strikewolf | 5:210cd333f770 | 13 | #define CHAN1_MIN 1210 //Rudder Lo |
Strikewolf | 5:210cd333f770 | 14 | |
Strikewolf | 5:210cd333f770 | 15 | //H-bridge |
Strikewolf | 5:210cd333f770 | 16 | PwmOut rightMotorPWM(p22); //Channel A |
Strikewolf | 5:210cd333f770 | 17 | PwmOut leftMotorPWM(p21); //Channel B |
Strikewolf | 5:210cd333f770 | 18 | DigitalOut leftMotor1(p23); |
Strikewolf | 5:210cd333f770 | 19 | DigitalOut leftMotor2(p24); |
Strikewolf | 5:210cd333f770 | 20 | DigitalOut rightMotor1(p25); |
Strikewolf | 5:210cd333f770 | 21 | DigitalOut rightMotor2(p26); |
Strikewolf | 5:210cd333f770 | 22 | |
Strikewolf | 5:210cd333f770 | 23 | //Angle state variable |
Strikewolf | 5:210cd333f770 | 24 | int rtheta; |
Strikewolf | 5:210cd333f770 | 25 | |
Strikewolf | 5:210cd333f770 | 26 | |
Strikewolf | 5:210cd333f770 | 27 | //Non-feedback corrected 'dumb' driving |
Strikewolf | 5:210cd333f770 | 28 | void setMove(float speed, bool reverse) |
Strikewolf | 5:210cd333f770 | 29 | { |
Strikewolf | 5:210cd333f770 | 30 | //Set speed |
Strikewolf | 5:210cd333f770 | 31 | rightMotorPWM = speed; |
Strikewolf | 5:210cd333f770 | 32 | leftMotorPWM = speed; |
Strikewolf | 5:210cd333f770 | 33 | |
Strikewolf | 5:210cd333f770 | 34 | //Set motor function |
Strikewolf | 5:210cd333f770 | 35 | leftMotor1 = (!reverse) ? 0 : 1; |
Strikewolf | 5:210cd333f770 | 36 | leftMotor2 = (!reverse) ? 1 : 0; |
Strikewolf | 5:210cd333f770 | 37 | rightMotor1 = (!reverse) ? 0 : 1; |
Strikewolf | 5:210cd333f770 | 38 | rightMotor2 = (!reverse) ? 1 : 0; |
Strikewolf | 5:210cd333f770 | 39 | } |
Strikewolf | 5:210cd333f770 | 40 | |
Strikewolf | 5:210cd333f770 | 41 | void setTurnLeft(float speed) |
Strikewolf | 5:210cd333f770 | 42 | { |
Strikewolf | 5:210cd333f770 | 43 | //Set speed |
Strikewolf | 5:210cd333f770 | 44 | rightMotorPWM = speed; |
Strikewolf | 5:210cd333f770 | 45 | leftMotorPWM = speed; |
Strikewolf | 5:210cd333f770 | 46 | |
Strikewolf | 5:210cd333f770 | 47 | //Set motor function |
Strikewolf | 5:210cd333f770 | 48 | leftMotor1 = 0; |
Strikewolf | 5:210cd333f770 | 49 | leftMotor2 = 1; |
Strikewolf | 5:210cd333f770 | 50 | rightMotor1 = 1; |
Strikewolf | 5:210cd333f770 | 51 | rightMotor2 = 0; |
Strikewolf | 5:210cd333f770 | 52 | } |
Strikewolf | 5:210cd333f770 | 53 | |
Strikewolf | 5:210cd333f770 | 54 | void setTurnRight(float speed) |
Strikewolf | 5:210cd333f770 | 55 | { |
Strikewolf | 5:210cd333f770 | 56 | //Set speed |
Strikewolf | 5:210cd333f770 | 57 | rightMotorPWM = speed; |
Strikewolf | 5:210cd333f770 | 58 | leftMotorPWM = speed; |
Strikewolf | 5:210cd333f770 | 59 | |
Strikewolf | 5:210cd333f770 | 60 | //Set motor function |
Strikewolf | 5:210cd333f770 | 61 | leftMotor1 = 1; |
Strikewolf | 5:210cd333f770 | 62 | leftMotor2 = 0; |
Strikewolf | 5:210cd333f770 | 63 | rightMotor1 = 0; |
Strikewolf | 5:210cd333f770 | 64 | rightMotor2 = 1; |
Strikewolf | 5:210cd333f770 | 65 | } |
Strikewolf | 5:210cd333f770 | 66 | |
Strikewolf | 5:210cd333f770 | 67 | //Stop with braking |
Strikewolf | 5:210cd333f770 | 68 | void stop() |
Strikewolf | 5:210cd333f770 | 69 | { |
Strikewolf | 5:210cd333f770 | 70 | rightMotorPWM = 0; |
Strikewolf | 5:210cd333f770 | 71 | leftMotorPWM = 0; |
Strikewolf | 5:210cd333f770 | 72 | leftMotor1 = 0; |
Strikewolf | 5:210cd333f770 | 73 | leftMotor2 = 1; |
Strikewolf | 5:210cd333f770 | 74 | rightMotor1 = 1; |
Strikewolf | 5:210cd333f770 | 75 | rightMotor2 = 0; |
Strikewolf | 5:210cd333f770 | 76 | } |
Strikewolf | 5:210cd333f770 | 77 | |
Strikewolf | 5:210cd333f770 | 78 | //Turn left about the center |
Strikewolf | 6:3fb9f96765f6 | 79 | void centerTurnLeft(bool gameRun) |
Strikewolf | 5:210cd333f770 | 80 | { |
Strikewolf | 5:210cd333f770 | 81 | //Reset the filter and re-init the IMU |
Strikewolf | 5:210cd333f770 | 82 | imuFilter.reset(); |
Strikewolf | 5:210cd333f770 | 83 | rpy_init(); |
Strikewolf | 6:3fb9f96765f6 | 84 | int delta_degrees = 0; |
Strikewolf | 5:210cd333f770 | 85 | |
Strikewolf | 5:210cd333f770 | 86 | //So there's this weird thing where the gyro treats left turns as POSITIVE degrees... |
Strikewolf | 5:210cd333f770 | 87 | float initial = toDegrees(imuFilter.getYaw()); |
Strikewolf | 5:210cd333f770 | 88 | float target = initial + delta_degrees; |
Strikewolf | 5:210cd333f770 | 89 | double sample; |
Strikewolf | 5:210cd333f770 | 90 | // pc.printf("Init: %f Target: %f\r\n", initial, target); |
Strikewolf | 5:210cd333f770 | 91 | |
Strikewolf | 5:210cd333f770 | 92 | //Continue to turn to target |
Strikewolf | 5:210cd333f770 | 93 | while (!gameOver) { |
Strikewolf | 5:210cd333f770 | 94 | setTurnLeft(0.6); |
Strikewolf | 6:3fb9f96765f6 | 95 | if(gameRun) { |
Strikewolf | 6:3fb9f96765f6 | 96 | receivePosition(NULL); |
Strikewolf | 6:3fb9f96765f6 | 97 | } |
Strikewolf | 5:210cd333f770 | 98 | sample = toDegrees(imuFilter.getYaw()); |
Strikewolf | 5:210cd333f770 | 99 | wait(0.02); |
Strikewolf | 6:3fb9f96765f6 | 100 | if((sonar3.read()*100) > (SONAR_STOP+6)) |
Strikewolf | 5:210cd333f770 | 101 | break; |
Strikewolf | 5:210cd333f770 | 102 | } |
Strikewolf | 5:210cd333f770 | 103 | stop(); |
Strikewolf | 5:210cd333f770 | 104 | } |
Strikewolf | 5:210cd333f770 | 105 | |
Strikewolf | 5:210cd333f770 | 106 | //Turn right about the center |
Strikewolf | 6:3fb9f96765f6 | 107 | void centerTurnRight(bool gameRun) |
Strikewolf | 5:210cd333f770 | 108 | { |
Strikewolf | 5:210cd333f770 | 109 | //Reset the filter and re-init the IMU |
Strikewolf | 5:210cd333f770 | 110 | imuFilter.reset(); |
Strikewolf | 5:210cd333f770 | 111 | rpy_init(); |
Strikewolf | 6:3fb9f96765f6 | 112 | |
Strikewolf | 6:3fb9f96765f6 | 113 | int delta_degrees = 0; |
Strikewolf | 5:210cd333f770 | 114 | |
Strikewolf | 5:210cd333f770 | 115 | //So there's this weird thing where the gyro treats right turns as NEGATIVE degrees... |
Strikewolf | 5:210cd333f770 | 116 | float initial = toDegrees(imuFilter.getYaw()); |
Strikewolf | 5:210cd333f770 | 117 | float target = initial - delta_degrees; |
Strikewolf | 5:210cd333f770 | 118 | double sample; |
Strikewolf | 5:210cd333f770 | 119 | |
Strikewolf | 5:210cd333f770 | 120 | // pc.printf("Init: %f Target: %f\r\n", initial, target); |
Strikewolf | 5:210cd333f770 | 121 | |
Strikewolf | 5:210cd333f770 | 122 | //Continue to turn to target |
Strikewolf | 5:210cd333f770 | 123 | while (!gameOver) { |
Strikewolf | 5:210cd333f770 | 124 | setTurnRight(0.6); |
Strikewolf | 6:3fb9f96765f6 | 125 | if(gameRun) { |
Strikewolf | 6:3fb9f96765f6 | 126 | receivePosition(NULL); |
Strikewolf | 6:3fb9f96765f6 | 127 | } |
Strikewolf | 5:210cd333f770 | 128 | sample = toDegrees(imuFilter.getYaw()); |
Strikewolf | 5:210cd333f770 | 129 | wait(0.02); |
Strikewolf | 6:3fb9f96765f6 | 130 | if((sonar1.read()*100) > (SONAR_STOP+6)) |
Strikewolf | 5:210cd333f770 | 131 | break; |
Strikewolf | 5:210cd333f770 | 132 | } |
Strikewolf | 5:210cd333f770 | 133 | stop(); |
Strikewolf | 5:210cd333f770 | 134 | } |
Strikewolf | 5:210cd333f770 | 135 | |
Strikewolf | 6:3fb9f96765f6 | 136 | void checkSonars(bool gameRun) |
Strikewolf | 5:210cd333f770 | 137 | { |
Strikewolf | 5:210cd333f770 | 138 | //Collision Detection |
Strikewolf | 5:210cd333f770 | 139 | // read all sonar sensors |
Strikewolf | 5:210cd333f770 | 140 | float s1 = sonar1.read() * 100; |
Strikewolf | 5:210cd333f770 | 141 | float s2 = sonar2.read() * 100; |
Strikewolf | 5:210cd333f770 | 142 | float s3 = sonar3.read() * 100; |
Strikewolf | 5:210cd333f770 | 143 | |
Strikewolf | 5:210cd333f770 | 144 | // check if obstacle is near and adjust |
Strikewolf | 6:3fb9f96765f6 | 145 | // pc.printf("%.2f, %.2f, %.2f, %f\n\r", s1, s2, s3, SONAR_STOP); |
Strikewolf | 5:210cd333f770 | 146 | if(s2 < SONAR_STOP) { |
Strikewolf | 5:210cd333f770 | 147 | |
Strikewolf | 5:210cd333f770 | 148 | if(s1 <= s3) { |
Strikewolf | 6:3fb9f96765f6 | 149 | centerTurnRight(gameRun); |
Strikewolf | 5:210cd333f770 | 150 | } else { |
Strikewolf | 6:3fb9f96765f6 | 151 | centerTurnLeft(gameRun); |
Strikewolf | 5:210cd333f770 | 152 | } |
Strikewolf | 6:3fb9f96765f6 | 153 | //imuFilter.reset(); |
Strikewolf | 6:3fb9f96765f6 | 154 | //rpy_init(); |
Strikewolf | 5:210cd333f770 | 155 | } |
Strikewolf | 5:210cd333f770 | 156 | } |
Strikewolf | 5:210cd333f770 | 157 | |
Strikewolf | 5:210cd333f770 | 158 | //Drive straight with compass correction |
Strikewolf | 5:210cd333f770 | 159 | void gyroDriveStraight(float speed, bool gameRun, int ms) |
Strikewolf | 5:210cd333f770 | 160 | { |
Strikewolf | 5:210cd333f770 | 161 | Timer timer; |
Strikewolf | 5:210cd333f770 | 162 | double sample; |
Strikewolf | 5:210cd333f770 | 163 | |
Strikewolf | 5:210cd333f770 | 164 | //Reset IMU |
Strikewolf | 5:210cd333f770 | 165 | imuFilter.reset(); |
Strikewolf | 5:210cd333f770 | 166 | rpy_init(); |
Strikewolf | 5:210cd333f770 | 167 | |
Strikewolf | 5:210cd333f770 | 168 | wait(0.2); |
Strikewolf | 5:210cd333f770 | 169 | |
Strikewolf | 5:210cd333f770 | 170 | timer.start(); |
Strikewolf | 5:210cd333f770 | 171 | while (timer.read_ms() < ms) { |
Strikewolf | 5:210cd333f770 | 172 | |
Strikewolf | 5:210cd333f770 | 173 | timer.stop(); |
Strikewolf | 6:3fb9f96765f6 | 174 | checkSonars(gameRun); |
Strikewolf | 5:210cd333f770 | 175 | if(gameRun) { |
Strikewolf | 5:210cd333f770 | 176 | receivePosition(NULL); |
Strikewolf | 5:210cd333f770 | 177 | } |
Strikewolf | 5:210cd333f770 | 178 | timer.start(); |
Strikewolf | 5:210cd333f770 | 179 | |
Strikewolf | 5:210cd333f770 | 180 | imuFilter.computeEuler(); |
Strikewolf | 5:210cd333f770 | 181 | sample = toDegrees(imuFilter.getYaw()); |
Strikewolf | 6:3fb9f96765f6 | 182 | |
Strikewolf | 5:210cd333f770 | 183 | //Drift Correction |
Strikewolf | 5:210cd333f770 | 184 | sample = sample + float(timer.read_ms() / 800); |
Strikewolf | 5:210cd333f770 | 185 | // pc.printf("%f :::", sample); |
Strikewolf | 5:210cd333f770 | 186 | |
Strikewolf | 5:210cd333f770 | 187 | if (sample < 3) { |
Strikewolf | 5:210cd333f770 | 188 | // pc.printf("Steer Left\r\n"); |
Strikewolf | 6:3fb9f96765f6 | 189 | leftMotorPWM = speed; |
Strikewolf | 5:210cd333f770 | 190 | rightMotorPWM = speed; |
Strikewolf | 5:210cd333f770 | 191 | leftMotor1 = 0; |
Strikewolf | 5:210cd333f770 | 192 | leftMotor2 = 1; |
Strikewolf | 5:210cd333f770 | 193 | rightMotor1 = 0; |
Strikewolf | 5:210cd333f770 | 194 | rightMotor2 = 1; |
Strikewolf | 5:210cd333f770 | 195 | } else if (sample > -3) { |
Strikewolf | 5:210cd333f770 | 196 | // pc.printf("Steer Right \r\n"); |
Strikewolf | 5:210cd333f770 | 197 | leftMotorPWM = speed; |
Strikewolf | 6:3fb9f96765f6 | 198 | rightMotorPWM = speed - 0.2; |
Strikewolf | 5:210cd333f770 | 199 | leftMotor1 = 0; |
Strikewolf | 5:210cd333f770 | 200 | leftMotor2 = 1; |
Strikewolf | 5:210cd333f770 | 201 | rightMotor1 = 0; |
Strikewolf | 5:210cd333f770 | 202 | rightMotor2 = 1; |
Strikewolf | 5:210cd333f770 | 203 | } else { |
Strikewolf | 5:210cd333f770 | 204 | // pc.printf("Straight\r\n"); |
Strikewolf | 5:210cd333f770 | 205 | setMove(speed, false); |
Strikewolf | 6:3fb9f96765f6 | 206 | } |
Strikewolf | 5:210cd333f770 | 207 | } |
Strikewolf | 5:210cd333f770 | 208 | stop(); |
Strikewolf | 5:210cd333f770 | 209 | } |
Strikewolf | 5:210cd333f770 | 210 | |
Strikewolf | 5:210cd333f770 | 211 | //Check where the evil honeywell-wielding human scum is and perform avoidance |
Strikewolf | 5:210cd333f770 | 212 | void avoidHoneywell() |
Strikewolf | 5:210cd333f770 | 213 | { |
Strikewolf | 5:210cd333f770 | 214 | //Grab telemetry from human car |
Strikewolf | 5:210cd333f770 | 215 | int human_x = x_hum; |
Strikewolf | 5:210cd333f770 | 216 | int human_y = y_hum; |
Strikewolf | 5:210cd333f770 | 217 | |
Strikewolf | 5:210cd333f770 | 218 | //Calculate avoidance angle |
Strikewolf | 5:210cd333f770 | 219 | int dx = x_position - human_x; |
Strikewolf | 5:210cd333f770 | 220 | int dy = y_position - human_y; |
Strikewolf | 5:210cd333f770 | 221 | |
Strikewolf | 5:210cd333f770 | 222 | //Set to current angle just in case everything screws up |
Strikewolf | 5:210cd333f770 | 223 | int desiredAngle = 0;// t_position; |
Strikewolf | 5:210cd333f770 | 224 | |
Strikewolf | 5:210cd333f770 | 225 | if (dx >= 0 && dy >= 0) |
Strikewolf | 5:210cd333f770 | 226 | desiredAngle = (dx > dy) ? 180 : 270; |
Strikewolf | 5:210cd333f770 | 227 | if (dx >= 0 && dy < 0) |
Strikewolf | 5:210cd333f770 | 228 | desiredAngle = (dx > dy) ? 180 : 90; |
Strikewolf | 5:210cd333f770 | 229 | if (dx < 0 && dy >= 0 ) |
Strikewolf | 5:210cd333f770 | 230 | desiredAngle = (dx > dy) ? 0 : 270; |
Strikewolf | 5:210cd333f770 | 231 | if (dx < 0 && dy < 0) |
Strikewolf | 5:210cd333f770 | 232 | desiredAngle = (dx > dy) ? 0 : 90; |
Strikewolf | 5:210cd333f770 | 233 | |
Strikewolf | 5:210cd333f770 | 234 | int delta_angle = 0;// t_position - desiredAngle; |
Strikewolf | 5:210cd333f770 | 235 | if (delta_angle >= 0) { |
Strikewolf | 5:210cd333f770 | 236 | //Right turn to some degree |
Strikewolf | 5:210cd333f770 | 237 | centerTurnRight(delta_angle); |
Strikewolf | 5:210cd333f770 | 238 | } else { |
Strikewolf | 5:210cd333f770 | 239 | centerTurnLeft(delta_angle); |
Strikewolf | 5:210cd333f770 | 240 | } |
Strikewolf | 5:210cd333f770 | 241 | } |