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@5:210cd333f770, 2014-04-30 (annotated)

Committer:
Strikewolf
Date:
Wed Apr 30 05:53:51 2014 +0000
Revision:
5:210cd333f770
Child:
6:3fb9f96765f6
works without any motors

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 5:210cd333f770 79 void centerTurnLeft(int delta_degrees)
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 5:210cd333f770 84
Strikewolf 5:210cd333f770 85 //So there's this weird thing where the gyro treats left turns as POSITIVE degrees...
Strikewolf 5:210cd333f770 86 float initial = toDegrees(imuFilter.getYaw());
Strikewolf 5:210cd333f770 87 float target = initial + delta_degrees;
Strikewolf 5:210cd333f770 88 double sample;
Strikewolf 5:210cd333f770 89 // pc.printf("Init: %f Target: %f\r\n", initial, target);
Strikewolf 5:210cd333f770 90
Strikewolf 5:210cd333f770 91 //Continue to turn to target
Strikewolf 5:210cd333f770 92 while (!gameOver) {
Strikewolf 5:210cd333f770 93 setTurnLeft(0.6);
Strikewolf 5:210cd333f770 94 sample = toDegrees(imuFilter.getYaw());
Strikewolf 5:210cd333f770 95 wait(0.02);
Strikewolf 5:210cd333f770 96 if(sample > target - 5)
Strikewolf 5:210cd333f770 97 break;
Strikewolf 5:210cd333f770 98 }
Strikewolf 5:210cd333f770 99 stop();
Strikewolf 5:210cd333f770 100 }
Strikewolf 5:210cd333f770 101
Strikewolf 5:210cd333f770 102 //Turn right about the center
Strikewolf 5:210cd333f770 103 void centerTurnRight(int delta_degrees)
Strikewolf 5:210cd333f770 104 {
Strikewolf 5:210cd333f770 105 //Reset the filter and re-init the IMU
Strikewolf 5:210cd333f770 106 imuFilter.reset();
Strikewolf 5:210cd333f770 107 rpy_init();
Strikewolf 5:210cd333f770 108
Strikewolf 5:210cd333f770 109 //So there's this weird thing where the gyro treats right turns as NEGATIVE degrees...
Strikewolf 5:210cd333f770 110 float initial = toDegrees(imuFilter.getYaw());
Strikewolf 5:210cd333f770 111 float target = initial - delta_degrees;
Strikewolf 5:210cd333f770 112 double sample;
Strikewolf 5:210cd333f770 113
Strikewolf 5:210cd333f770 114 // pc.printf("Init: %f Target: %f\r\n", initial, target);
Strikewolf 5:210cd333f770 115
Strikewolf 5:210cd333f770 116 //Continue to turn to target
Strikewolf 5:210cd333f770 117 while (!gameOver) {
Strikewolf 5:210cd333f770 118 setTurnRight(0.6);
Strikewolf 5:210cd333f770 119 sample = toDegrees(imuFilter.getYaw());
Strikewolf 5:210cd333f770 120 wait(0.02);
Strikewolf 5:210cd333f770 121 if(sample < target)
Strikewolf 5:210cd333f770 122 break;
Strikewolf 5:210cd333f770 123 }
Strikewolf 5:210cd333f770 124 stop();
Strikewolf 5:210cd333f770 125 }
Strikewolf 5:210cd333f770 126
Strikewolf 5:210cd333f770 127 void checkSonars()
Strikewolf 5:210cd333f770 128 {
Strikewolf 5:210cd333f770 129 //Collision Detection
Strikewolf 5:210cd333f770 130 // read all sonar sensors
Strikewolf 5:210cd333f770 131 float s1 = sonar1.read() * 100;
Strikewolf 5:210cd333f770 132 float s2 = sonar2.read() * 100;
Strikewolf 5:210cd333f770 133 float s3 = sonar3.read() * 100;
Strikewolf 5:210cd333f770 134
Strikewolf 5:210cd333f770 135 // check if obstacle is near and adjust
Strikewolf 5:210cd333f770 136 pc.printf("%.2f, %.2f, %.2f, %f\n\r", s1, s2, s3, SONAR_STOP);
Strikewolf 5:210cd333f770 137 if(s2 < SONAR_STOP) {
Strikewolf 5:210cd333f770 138
Strikewolf 5:210cd333f770 139 if(s1 <= s3) {
Strikewolf 5:210cd333f770 140 centerTurnRight(90);
Strikewolf 5:210cd333f770 141 } else {
Strikewolf 5:210cd333f770 142 centerTurnLeft(90);
Strikewolf 5:210cd333f770 143 }
Strikewolf 5:210cd333f770 144 imuFilter.reset();
Strikewolf 5:210cd333f770 145 rpy_init();
Strikewolf 5:210cd333f770 146 }
Strikewolf 5:210cd333f770 147 }
Strikewolf 5:210cd333f770 148
Strikewolf 5:210cd333f770 149 //Drive straight with compass correction
Strikewolf 5:210cd333f770 150 void gyroDriveStraight(float speed, bool gameRun, int ms)
Strikewolf 5:210cd333f770 151 {
Strikewolf 5:210cd333f770 152 Timer timer;
Strikewolf 5:210cd333f770 153 double sample;
Strikewolf 5:210cd333f770 154
Strikewolf 5:210cd333f770 155 //Reset IMU
Strikewolf 5:210cd333f770 156 imuFilter.reset();
Strikewolf 5:210cd333f770 157 rpy_init();
Strikewolf 5:210cd333f770 158
Strikewolf 5:210cd333f770 159 wait(0.2);
Strikewolf 5:210cd333f770 160
Strikewolf 5:210cd333f770 161 timer.start();
Strikewolf 5:210cd333f770 162 while (timer.read_ms() < ms) {
Strikewolf 5:210cd333f770 163
Strikewolf 5:210cd333f770 164 timer.stop();
Strikewolf 5:210cd333f770 165 //checkSonars();
Strikewolf 5:210cd333f770 166 if(gameRun) {
Strikewolf 5:210cd333f770 167 receivePosition(NULL);
Strikewolf 5:210cd333f770 168 }
Strikewolf 5:210cd333f770 169 timer.start();
Strikewolf 5:210cd333f770 170
Strikewolf 5:210cd333f770 171 imuFilter.computeEuler();
Strikewolf 5:210cd333f770 172 sample = toDegrees(imuFilter.getYaw());
Strikewolf 5:210cd333f770 173 /*
Strikewolf 5:210cd333f770 174 //Drift Correction
Strikewolf 5:210cd333f770 175 sample = sample + float(timer.read_ms() / 800);
Strikewolf 5:210cd333f770 176 // pc.printf("%f :::", sample);
Strikewolf 5:210cd333f770 177
Strikewolf 5:210cd333f770 178 if (sample < 3) {
Strikewolf 5:210cd333f770 179 // pc.printf("Steer Left\r\n");
Strikewolf 5:210cd333f770 180 leftMotorPWM = speed - 0.2;
Strikewolf 5:210cd333f770 181 rightMotorPWM = speed;
Strikewolf 5:210cd333f770 182 leftMotor1 = 0;
Strikewolf 5:210cd333f770 183 leftMotor2 = 1;
Strikewolf 5:210cd333f770 184 rightMotor1 = 0;
Strikewolf 5:210cd333f770 185 rightMotor2 = 1;
Strikewolf 5:210cd333f770 186 } else if (sample > -3) {
Strikewolf 5:210cd333f770 187 // pc.printf("Steer Right \r\n");
Strikewolf 5:210cd333f770 188 leftMotorPWM = speed;
Strikewolf 5:210cd333f770 189 rightMotorPWM = speed - 0.18;
Strikewolf 5:210cd333f770 190 leftMotor1 = 0;
Strikewolf 5:210cd333f770 191 leftMotor2 = 1;
Strikewolf 5:210cd333f770 192 rightMotor1 = 0;
Strikewolf 5:210cd333f770 193 rightMotor2 = 1;
Strikewolf 5:210cd333f770 194 } else {
Strikewolf 5:210cd333f770 195 // pc.printf("Straight\r\n");
Strikewolf 5:210cd333f770 196 setMove(speed, false);
Strikewolf 5:210cd333f770 197 }*/
Strikewolf 5:210cd333f770 198 }
Strikewolf 5:210cd333f770 199 stop();
Strikewolf 5:210cd333f770 200 }
Strikewolf 5:210cd333f770 201
Strikewolf 5:210cd333f770 202 //Check where the evil honeywell-wielding human scum is and perform avoidance
Strikewolf 5:210cd333f770 203 void avoidHoneywell()
Strikewolf 5:210cd333f770 204 {
Strikewolf 5:210cd333f770 205 //Grab telemetry from human car
Strikewolf 5:210cd333f770 206 int human_x = x_hum;
Strikewolf 5:210cd333f770 207 int human_y = y_hum;
Strikewolf 5:210cd333f770 208
Strikewolf 5:210cd333f770 209 //Calculate avoidance angle
Strikewolf 5:210cd333f770 210 int dx = x_position - human_x;
Strikewolf 5:210cd333f770 211 int dy = y_position - human_y;
Strikewolf 5:210cd333f770 212
Strikewolf 5:210cd333f770 213 //Set to current angle just in case everything screws up
Strikewolf 5:210cd333f770 214 int desiredAngle = 0;// t_position;
Strikewolf 5:210cd333f770 215
Strikewolf 5:210cd333f770 216 if (dx >= 0 && dy >= 0)
Strikewolf 5:210cd333f770 217 desiredAngle = (dx > dy) ? 180 : 270;
Strikewolf 5:210cd333f770 218 if (dx >= 0 && dy < 0)
Strikewolf 5:210cd333f770 219 desiredAngle = (dx > dy) ? 180 : 90;
Strikewolf 5:210cd333f770 220 if (dx < 0 && dy >= 0 )
Strikewolf 5:210cd333f770 221 desiredAngle = (dx > dy) ? 0 : 270;
Strikewolf 5:210cd333f770 222 if (dx < 0 && dy < 0)
Strikewolf 5:210cd333f770 223 desiredAngle = (dx > dy) ? 0 : 90;
Strikewolf 5:210cd333f770 224
Strikewolf 5:210cd333f770 225 int delta_angle = 0;// t_position - desiredAngle;
Strikewolf 5:210cd333f770 226 if (delta_angle >= 0) {
Strikewolf 5:210cd333f770 227 //Right turn to some degree
Strikewolf 5:210cd333f770 228 centerTurnRight(delta_angle);
Strikewolf 5:210cd333f770 229 } else {
Strikewolf 5:210cd333f770 230 centerTurnLeft(delta_angle);
Strikewolf 5:210cd333f770 231 }
Strikewolf 5:210cd333f770 232 }