File content as of revision 6:3fb9f96765f6:

//*******************************************
//* 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(bool gameRun)
{
    //Reset the filter and re-init the IMU
    imuFilter.reset();
    rpy_init();
    int delta_degrees = 0;

    //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);
        if(gameRun) {
            receivePosition(NULL);
        }
        sample = toDegrees(imuFilter.getYaw());
        wait(0.02);
        if((sonar3.read()*100) > (SONAR_STOP+6))
            break;
    }
    stop();
}

//Turn right about the center
void centerTurnRight(bool gameRun)
{
    //Reset the filter and re-init the IMU
    imuFilter.reset();
    rpy_init();
    
    int delta_degrees = 0;

    //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);
        if(gameRun) {
            receivePosition(NULL);
        }
        sample = toDegrees(imuFilter.getYaw());
        wait(0.02);
        if((sonar1.read()*100) > (SONAR_STOP+6))
            break;
    }
    stop();
}

void checkSonars(bool gameRun)
{
    //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(gameRun);
        } else {
            centerTurnLeft(gameRun);
        }
        //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(gameRun);
        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;
            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.2;
            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);
    }
}