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
- Committer:
- Strikewolf
- Date:
- 2014-04-30
- Revision:
- 5:210cd333f770
- Child:
- 6:3fb9f96765f6
File content as of revision 5:210cd333f770:
//******************************************* //* Robot motor control and drive functions * //******************************************* #define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms //Radio Calibration Signals - Need to implement calibration routine... #define CHAN3_MAX 2060 //Throttle Hi #define CHAN3_MIN 1150 ` //Throttle Lo #define CHAN2_MAX 1260 //Elevator Hi #define CHAN2_MIN 2290 //Elevator Lo #define CHAN1_MAX 2160 //Rudder Hi #define CHAN1_MIN 1210 //Rudder Lo //H-bridge PwmOut rightMotorPWM(p22); //Channel A PwmOut leftMotorPWM(p21); //Channel B DigitalOut leftMotor1(p23); DigitalOut leftMotor2(p24); DigitalOut rightMotor1(p25); DigitalOut rightMotor2(p26); //Angle state variable int rtheta; //Non-feedback corrected 'dumb' driving void setMove(float speed, bool reverse) { //Set speed rightMotorPWM = speed; leftMotorPWM = speed; //Set motor function leftMotor1 = (!reverse) ? 0 : 1; leftMotor2 = (!reverse) ? 1 : 0; rightMotor1 = (!reverse) ? 0 : 1; rightMotor2 = (!reverse) ? 1 : 0; } void setTurnLeft(float speed) { //Set speed rightMotorPWM = speed; leftMotorPWM = speed; //Set motor function leftMotor1 = 0; leftMotor2 = 1; rightMotor1 = 1; rightMotor2 = 0; } void setTurnRight(float speed) { //Set speed rightMotorPWM = speed; leftMotorPWM = speed; //Set motor function leftMotor1 = 1; leftMotor2 = 0; rightMotor1 = 0; rightMotor2 = 1; } //Stop with braking void stop() { rightMotorPWM = 0; leftMotorPWM = 0; leftMotor1 = 0; leftMotor2 = 1; rightMotor1 = 1; rightMotor2 = 0; } //Turn left about the center void centerTurnLeft(int delta_degrees) { //Reset the filter and re-init the IMU imuFilter.reset(); rpy_init(); //So there's this weird thing where the gyro treats left turns as POSITIVE degrees... float initial = toDegrees(imuFilter.getYaw()); float target = initial + delta_degrees; double sample; // pc.printf("Init: %f Target: %f\r\n", initial, target); //Continue to turn to target while (!gameOver) { setTurnLeft(0.6); sample = toDegrees(imuFilter.getYaw()); wait(0.02); if(sample > target - 5) break; } stop(); } //Turn right about the center void centerTurnRight(int delta_degrees) { //Reset the filter and re-init the IMU imuFilter.reset(); rpy_init(); //So there's this weird thing where the gyro treats right turns as NEGATIVE degrees... float initial = toDegrees(imuFilter.getYaw()); float target = initial - delta_degrees; double sample; // pc.printf("Init: %f Target: %f\r\n", initial, target); //Continue to turn to target while (!gameOver) { setTurnRight(0.6); sample = toDegrees(imuFilter.getYaw()); wait(0.02); if(sample < target) break; } stop(); } void checkSonars() { //Collision Detection // read all sonar sensors float s1 = sonar1.read() * 100; float s2 = sonar2.read() * 100; float s3 = sonar3.read() * 100; // check if obstacle is near and adjust pc.printf("%.2f, %.2f, %.2f, %f\n\r", s1, s2, s3, SONAR_STOP); if(s2 < SONAR_STOP) { if(s1 <= s3) { centerTurnRight(90); } else { centerTurnLeft(90); } imuFilter.reset(); rpy_init(); } } //Drive straight with compass correction void gyroDriveStraight(float speed, bool gameRun, int ms) { Timer timer; double sample; //Reset IMU imuFilter.reset(); rpy_init(); wait(0.2); timer.start(); while (timer.read_ms() < ms) { timer.stop(); //checkSonars(); if(gameRun) { receivePosition(NULL); } timer.start(); imuFilter.computeEuler(); sample = toDegrees(imuFilter.getYaw()); /* //Drift Correction sample = sample + float(timer.read_ms() / 800); // pc.printf("%f :::", sample); if (sample < 3) { // pc.printf("Steer Left\r\n"); leftMotorPWM = speed - 0.2; rightMotorPWM = speed; leftMotor1 = 0; leftMotor2 = 1; rightMotor1 = 0; rightMotor2 = 1; } else if (sample > -3) { // pc.printf("Steer Right \r\n"); leftMotorPWM = speed; rightMotorPWM = speed - 0.18; leftMotor1 = 0; leftMotor2 = 1; rightMotor1 = 0; rightMotor2 = 1; } else { // pc.printf("Straight\r\n"); setMove(speed, false); }*/ } stop(); } //Check where the evil honeywell-wielding human scum is and perform avoidance void avoidHoneywell() { //Grab telemetry from human car int human_x = x_hum; int human_y = y_hum; //Calculate avoidance angle int dx = x_position - human_x; int dy = y_position - human_y; //Set to current angle just in case everything screws up int desiredAngle = 0;// t_position; if (dx >= 0 && dy >= 0) desiredAngle = (dx > dy) ? 180 : 270; if (dx >= 0 && dy < 0) desiredAngle = (dx > dy) ? 180 : 90; if (dx < 0 && dy >= 0 ) desiredAngle = (dx > dy) ? 0 : 270; if (dx < 0 && dy < 0) desiredAngle = (dx > dy) ? 0 : 90; int delta_angle = 0;// t_position - desiredAngle; if (delta_angle >= 0) { //Right turn to some degree centerTurnRight(delta_angle); } else { centerTurnLeft(delta_angle); } }