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
Diff: AIControl.h
- Revision:
- 5:210cd333f770
- Child:
- 6:3fb9f96765f6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AIControl.h Wed Apr 30 05:53:51 2014 +0000 @@ -0,0 +1,232 @@ +//******************************************* +//* 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); + } +}