Defense(Safe).c

#pragma config(Hubs,  S3, HTMotor,  none,     none,     none)
#pragma config(Sensor, S3,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S3_C1_1,     leftDrive,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S3_C1_2,     rightDrive,    tmotorTetrix, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
void forward(float distance);
void backward(float distance);
void initializeRobot();
task sudoMain();
task originalPath()
{
	forward(400);
	StopTask(sudoMain);
}
task defend() {
		StopTask(sudoMain);
		motor[leftDrive] = 0;
		motor[rightDrive] = 0;
		nMotorEncoder[leftDrive] = 0;
		nMotorEncoder[rightDrive] = 0;
		backward(10);
    StartTask(sudoMain);
}
task sudoMain()
{
		StopTask(defend);
    const float CRASH_THRESHOLD = 0.75; // 70% - 80% works
    ClearTimer(T1);
    ClearTimer(T2);
    ClearTimer(T3);
    ClearTimer(T4);
    nMotorEncoder[leftDrive]  = 0;
    nMotorEncoder[rightDrive] = 0;
    StartTask(originalPath);
    wait1Msec(1000);
    while(true)
    {
        float averageMotorSpeed = abs((motor[leftDrive] + motor[rightDrive]) / 2.0);
        float expectedValue = (-0.1801*(averageMotorSpeed*averageMotorSpeed)) + (43.294*(averageMotorSpeed));
        float actualValue = (abs(nMotorEncoder[leftDrive]) + abs(nMotorEncoder[rightDrive])) / 2.0;
        if(actualValue < (expectedValue * CRASH_THRESHOLD))
        {
            StopTask(originalPath);
            StartTask(defend);
            break;
        }
        nMotorEncoder[leftDrive]  = 0;
        nMotorEncoder[rightDrive] = 0;
        wait1Msec(1000);
    }
    while(true) { }
}
task main() {
	initializeRobot();
   //waitForStart();
	StartTask(sudoMain);
	while(true){}
}
void forward(float distance) {//forward function to pass in a length in inches and then it goes that far
    const float CHANGE = 2;
    float totalTraveled = 0;
    float encoderTarget = (169.92*distance) - 55.875; //calculate the encoder target
    nMotorEncoder[rightDrive] = 0;
    nMotorEncoder[leftDrive]  = 0;
    float leftEncoder;
    float rightEncoder;
    float leftPower  = 50;
    float rightPower = 50;
    while(abs(totalTraveled) < abs(encoderTarget))
    {
        leftEncoder  = nMotorEncoder[leftDrive];
        rightEncoder = nMotorEncoder[rightDrive];
        if(leftEncoder > rightEncoder)//changes based on which one has traveled farther
        {
            leftPower  -= CHANGE;
            rightPower += CHANGE;
        }
        else if(leftEncoder < rightEncoder)//same thing
        {
            leftPower  += CHANGE;
            rightPower -= CHANGE;
        }
        motor[leftDrive]  = leftPower;
        motor[rightDrive] = rightPower;
        totalTraveled += (leftEncoder + rightEncoder)/ 2.0;
        nMotorEncoder[rightDrive] = 0;
        nMotorEncoder[leftDrive]  = 0;
        ClearTimer(T1);
        while(time1[T1] < 200) {
            leftEncoder  = nMotorEncoder[leftDrive];
            rightEncoder = nMotorEncoder[rightDrive];
            if(!((abs(totalTraveled + (leftEncoder + rightEncoder)/ 2.0)) < abs(encoderTarget)))
            {
                totalTraveled += (nMotorEncoder[leftDrive] + nMotorEncoder[rightDrive])/ 2.0;
                break;
            }
        }
    }
}
void backward(float distance) {
    const float CHANGE = 2;
    float totalTraveled = 0;
    float encoderTarget = (169.92*distance) - 55.875;
    nMotorEncoder[rightDrive] = 0;
    nMotorEncoder[leftDrive]  = 0;
    float leftEncoder;
    float rightEncoder;
    float leftPower  = -50;
    float rightPower = -50;
    while(abs(totalTraveled) < abs(encoderTarget))
    {
        leftEncoder  = abs(nMotorEncoder[leftDrive]);
        rightEncoder = abs(nMotorEncoder[rightDrive]);
        if(leftEncoder > rightEncoder)
        {
            leftPower  -= CHANGE;
            rightPower += CHANGE;
        }
        else if(leftEncoder < rightEncoder)
        {
            leftPower  += CHANGE;
            rightPower -= CHANGE;
        }
        motor[leftDrive]  = leftPower;
        motor[rightDrive] = rightPower;
        totalTraveled += (leftEncoder + rightEncoder)/ 2.0;
        nMotorEncoder[rightDrive] = 0;
        nMotorEncoder[leftDrive]  = 0;
        ClearTimer(T1);
        while(time1[T1] < 200) {
            leftEncoder  = nMotorEncoder[leftDrive];
            rightEncoder = nMotorEncoder[rightDrive];
            if(!((abs(totalTraveled + (leftEncoder + rightEncoder)/ 2.0)) < abs(encoderTarget)))
            {
                totalTraveled += (nMotorEncoder[leftDrive] + nMotorEncoder[rightDrive])/ 2.0;
                break;
            }
        }
    }
}
void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}