Justin Eng / Mbed 2 deprecated RoboticsCatAndMouse_HumanDriver

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 Ganesh Subramaniam

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?

UserRevisionLine numberNew 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 }