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);
+    }
+}