XBot.c

#pragma config(Sensor, in1,    PRT_skillsJumper, sensorAnalog)
#pragma config(Sensor, dgtl1,  PRT_gunLeftQuad, sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  PRT_gunRightQuad, sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  PRT_leftSonar,  sensorSONAR_raw)
#pragma config(Sensor, dgtl7,  PRT_rightSonar, sensorSONAR_raw)
#pragma config(Sensor, dgtl9,  PRT_ledGun,     sensorLEDtoVCC)
#pragma config(Sensor, dgtl10, PRT_ledR,       sensorLEDtoVCC)
#pragma config(Sensor, dgtl11, PRT_ledY,       sensorLEDtoVCC)
#pragma config(Sensor, dgtl12, PRT_ledG,       sensorLEDtoVCC)
#pragma config(Motor,  port1,           PRT_feedLower, tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           PRT_feedUpper, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           PRT_wheelsRight, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           PRT_wheelsLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           PRT_gunRight1, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           PRT_gunRight2, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           PRT_gunRight3, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           PRT_gunLeft1,  tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           PRT_gunLeft2,  tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          PRT_gunLeft3,  tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Old configuration.
//#pragma config(Motor,  port1,           PRT_feedLower, tmotorVex393_HBridge, openLoop, reversed)
//#pragma config(Motor,  port2,           PRT_feedUpper, tmotorVex393_MC29, openLoop)
//#pragma config(Motor,  port3,           PRT_wheelFrontRight, tmotorVex393_MC29, openLoop, reversed)
//#pragma config(Motor,  port4,           PRT_wheelBackLeft, tmotorVex393_MC29, openLoop)
//#pragma config(Motor,  port5,           PRT_wheelBackRight, tmotorVex393_MC29, openLoop, reversed)
//#pragma config(Motor,  port6,           PRT_gunLeft1,  tmotorVex393_MC29, openLoop)
//#pragma config(Motor,  port7,           PRT_gunLeft2,  tmotorVex393_MC29, openLoop)
//#pragma config(Motor,  port8,           PRT_gunRight1, tmotorVex393_MC29, openLoop, reversed)
//#pragma config(Motor,  port9,           PRT_gunRight2, tmotorVex393_MC29, openLoop, reversed)
//#pragma config(Motor,  port10,          PRT_wheelFrontLeft, tmotorVex393_HBridge, openLoop)
//////////////////////////////////////////////////////////////////////////////////////
//                                  --= X BOT =--                                   //
//////////////////////////////////////////////////////////////////////////////////////
// The purpose of this file is to link all of the modules together into a working,  //
// intelligent robot. The robot starts by calibrating its systems, then enables its //
// autonomous mode. After this, user input is converted to instructions during the  //
// user control period.                                                             //
//////////////////////////////////////////////////////////////////////////////////////

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(120)

//Main competition background code...do not modify!
#include "Vex_Competition_Includes.c"

#define DEBUG 0

#define ROBOT "Robot"
#define EMULATOR "Emulator"
#define PROGRAMMING_CHALLENGE 0

#if (_TARGET == EMULATOR)
#warning "###!!!### RUNNING IN EMULATOR MODE ###!!!###"
#define EMULATING 1
#else
#define EMULATING 0
#endif

#if (DEBUG == 1)
#warning "###!!!### RUNNING IN DEBUG MODE ###!!!###"
#endif

//////////////////
// DEPENDENCIES //
//////////////////

#include "Utilities.c"
#include "FiringTable.c"
#include "BlinkModule.c"
#include "BatteryModule.c"
#include "SonarModule.c"
#include "TrajectoryModule.c"
#include "DriverControlModule.c"
#include "GunModule.c"
#include "PIDLoopModule.c"
#include "AutonomousModule.c"

///////////////
// TODO LIST //
/*/////////////

Install dummy jumper cable.
Test new autonomous.
Monitor battery backup.
Investigate power expander status port.

And as always, DOCUMENT MORE.

*///////////////////
// USER VARIABLES //
////////////////////

const float USR_DEFAULT_SPEED = 1475;
const float USR_DEFAULT_RANGE = 13 * 12;
const float USR_RANGE_LARGE_INCREMENT = 2 * 12; // 1 floor tile.
const float USR_RANGE_SMALL_INCREMENT = 0.5 * 12; // Quarter floor tile.
short USR_pingId = -1;
short USR_sonarId = -1;
short USR_reverseMultiplier = 1; // Setup the wheel reversal.
float USR_targetRange = USR_DEFAULT_RANGE; // In feet, how far the ball should go if fired.
float USR_targetSpeed = USR_DEFAULT_SPEED;
bool USR_programmingChallenge = false;

//////////////////////////
// PRE-AUTONOMOUS SETUP //
//////////////////////////

void pre_auton()
{
	writeDebugStreamLine("[Mode]: Setting up the robot...");
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = false; // This must be disabled to allow the PID Loop.

	// Reset Timer 1.
	clearTimer(T1);

	// Initialize the blink debug handler.
	// Each blink task has an ID that must be held in order to stop the task later.
	startTask(LED_blink);
	short id = LED_startBlinkTask(Info, Medium);

	// Initialize the driver configuration and button "event" handler.
	startTask(DRV_buttonHandler);

	// Initialize the battery monitor.
	startTask(BAT_monitor);

	// Check the programming skills jumper.
	if (SensorValue[PRT_skillsJumper] == 0) {
			USR_programmingChallenge = true;
			writeDebugStreamLine("[Mode]: Programming skills jumper detected. Autonomous program switched.");
	}	else {
			USR_programmingChallenge = false;
	}

	// Reset sensors.
	SensorType[PRT_gunLeftQuad] = sensorNone; // JPearman said to do this.
	SensorType[PRT_gunLeftQuad] = sensorQuadEncoder; // Resets encoders in RobotC, fixes error.
	SensorType[PRT_gunRightQuad] = sensorNone;
	SensorType[PRT_gunRightQuad] = sensorQuadEncoder;
	SensorValue[PRT_gunLeftQuad] = 0;
	SensorValue[PRT_gunRightQuad] = 0;

	// Start sonar updates.
	startTask(SNR_update);

	// Initialize the gun controller.
	startTask(GUN_controller);

	// Initialize the PID loop.
	startTask(PID_controller);

	// Prepare the Autonomous.
	//AUT_calibrate();

	// Enable the lower feed for 8 seconds to preload balls.
	//AUT_feedLower(127, 15);

	wait1Msec(500);
	LED_stopBlinkTask(id);
	writeDebugStreamLine("[Mode]: Setup complete!");
#if (EMULATING == 1)
	startTask(usercontrol);
#endif
}

/////////////////////
// AUTONOMOUS MODE //
/////////////////////

task autonomous()
{
	// Check the programming skills jumper. Again.
	if (SensorValue[PRT_skillsJumper] == 0) {
			USR_programmingChallenge = true;
			writeDebugStreamLine("[Mode]: Programming skills jumper detected. Autonomous program switched.");
	}	else {
			USR_programmingChallenge = false;
	}

	if (USR_programmingChallenge == false) {
		// COMPETITION AUTONOMOUS //
		short id = LED_startBlinkTask(Info, Slow);
		writeDebugStreamLine("[Mode]: Autonomous mode enabled!");
		writeDebugStreamLine("[Auton]: Warming guns...");
		PID_target[Left] = USR_DEFAULT_SPEED;
		PID_target[Right] = PID_target[Left];
		for (int i = 1; i <= 5; i++) { // In the event that one fails, add one more.
				while (!PID_ready) {
					wait1Msec(50);
				}
				writeDebugStreamLine("[Auton]: Firing ball %i...", i);
				AUT_feedLower(127);
				AUT_feedUpper(127);
				while (PID_ballFired == false) {
					wait1Msec(300);
				}
				PID_ballFired = false;
				writeDebugStreamLine("[Auton]: Ball fired!");
				AUT_feedLower(0);
				AUT_feedUpper(0);
		}
		writeDebugStreamLine("[Mode]: Autonomous mode disabled.");
		AUT_shutDown();
		LED_stopBlinkTask(id);
	} else {
		// PROGRAMMING SKILLS CHALLENGE //
		float startTime = ((float)time1[T1]) / 1000;
		short id = LED_startBlinkTask(Info, Slow);
		writeDebugStreamLine("[Mode]: Autonomous (programming skills) mode enabled!");
		writeDebugStreamLine("[Auton]: Warming guns...");
		PID_target[Left] = AUT_SHORT_SPEED;
		PID_target[Right] = PID_target[Left];
		// Wait for the gun to fully warm up.
		AUT_feedUpper(32);
		while (!PID_ready) {
			wait1Msec(50);
		}
		AUT_feedUpper(0);
		startTask(AUT_countFiredBalls);
		writeDebugStreamLine("[Auton]: Ready to fire.");
		float overrideTime = 35.0; // Leave the square after 35 seconds are up.
		float startHomeFiringTime = ((float)time1[T1]) / 1000;
		while (true) {
			if ((((float)time1[T1]) / 1000) - startTime >= overrideTime) {// || AUT_ballsFired >= 32) {
				// If 32 balls have been fired, exit loop to navigate to other side.
				// If override time exceeded, exit.
				writeDebugStreamLine("[Auton]: Exiting base to navigate to the opposite side...");
				AUT_feedUpper(32);
				break;
			}
			wait1Msec(50);
		}
		float navStartTime = ((float)time1[T1]) / 1000;
		writeDebugStreamLine("[Auton]: Balls were fired from home for %f seconds.", navStartTime - startHomeFiringTime);

		// To keep balls out, although unlikely.
		AUT_feedLower(-64);

		// Start rotating to prepare to reverse into opposite side.
		AUT_rotate(64, 1.4);
		writeDebugStreamLine("[Auton]: Moonwalking...");
		AUT_surge(-127, 3.2);
		writeDebugStreamLine("[Auton]: Correcting course...");
		// Start aligning with opposing wall.
		while (AUT_alignWithSonar(SNR_distanceInchesLeft, SNR_distanceInchesRight, 0.25) == false) {
			writeDebugStreamLine("Left: %f | Right: %f", SNR_distanceInchesLeft, SNR_distanceInchesRight);
			wait1Msec(50);
		}
		writeDebugStreamLine("Left: %f | Right: %f", SNR_distanceInchesLeft, SNR_distanceInchesRight);

		// Do the moonwalk. Just for show. (Reverse)
		writeDebugStreamLine("[Auton]: Course corrected. Continuing moonwalk...");
		AUT_surge(-127);
		wait1Msec(1700);
		writeDebugStreamLine("[Auton]: Average sonar distance is currently reading %f", (SNR_distanceInchesLeft + SNR_distanceInchesRight) / 2);
		while ((SNR_distanceInchesLeft + SNR_distanceInchesRight) / 2 >= 13.0
			&& SNR_distanceInchesLeft != SNR_INVALID && SNR_distanceInchesRight != SNR_INVALID) {
			writeDebugStreamLine("Average: %f", (SNR_distanceInchesLeft + SNR_distanceInchesRight) / 2);
			wait1Msec(50);
		}
		writeDebugStreamLine("Average: %f", (SNR_distanceInchesLeft + SNR_distanceInchesRight) / 2);
		// Stop after (average) 3 inches from wall.
		writeDebugStreamLine("[Auton]: Stopping and aligning with net...");
		AUT_halt();

		// Align with wall to face the new goal.
		AUT_rotate(-64, 1);
		while (AUT_alignWithSonar(SNR_distanceInchesLeft, SNR_distanceInchesRight, 0.5) == false) {
			wait1Msec(50);
		}
		// Prepare to fire everything!
		writeDebugStreamLine("[Auton]: Aligned with net. Correcting distance.");

		writeDebugStreamLine("[Auton]: Average sonar distance is currently reading %f", (SNR_distanceInchesLeft + SNR_distanceInchesRight) / 2);
		while (AUT_distanceWithSonar(SNR_distanceInchesLeft, SNR_distanceInchesRight, 25.0, 0.25) == false) {
			writeDebugStreamLine("Average: %f", (SNR_distanceInchesLeft + SNR_distanceInchesRight) / 2);
			wait1Msec(50);
		}

		writeDebugStreamLine("[Auton]: Distance corrected. Fine tuning angle...");

		while (AUT_alignWithSonar(SNR_distanceInchesLeft, SNR_distanceInchesRight, 0.05, 1.005) == false) {
			writeDebugStreamLine("Left: %f | Right: %f", SNR_distanceInchesLeft, SNR_distanceInchesRight);
			wait1Msec(50);
		}
		writeDebugStreamLine("[Auton]: Angle fine tuned. Ready to fire.");
		AUT_feedLower(0);
		AUT_feedUpper(0);

		writeDebugStreamLine("[Auton]: Navigation took %f seconds", (((float)time1[T1]) / 1000) - navStartTime);
		// Might want to tweak the end time so that we get every last millisecond out of the referee's reflex time.
		while ((((float)time1[T1]) / 1000) - startTime < 65.0) {
			wait1Msec(50);
		}
		writeDebugStreamLine("[Mode]: Autonomous (programming skills) mode disabled.");
		stopTask(AUT_countFiredBalls);
		AUT_shutDown();
		LED_stopBlinkTask(id);
	}
}

///////////////////////////
//// USER CONTROL MODE ////
///////////////////////////

// For manual, computer-based value editing.
bool USR_OVERRIDE_USER_CONTROL = false;
bool USR_FORCE_AUTON = false;
#if (DEBUG == 1)
USR_OVERRIDE_USER_CONTROL = true;
#endif

task usercontrol()
{
	writeDebugStreamLine("[Mode]: User Control mode enabled!");
	// Reset the PID target.
	PID_target[Left] = 0;
	PID_target[Right] = 0;
	// Make sure the ball count task was stopped.
	stopTask(AUT_countFiredBalls); // Reset ball counter.
	startTask(AUT_countFiredBalls);
	// Continually check for user input.
	while (!USR_OVERRIDE_USER_CONTROL) {
		// Feed Control
		if ((DRV_config[FeedLowerIn] != UNASSIGNED && vexRT[DRV_config[FeedLowerIn]] == true) ||
			(DRV_config[FeedLowerInSecondary] != UNASSIGNED && vexRT[DRV_config[FeedLowerInSecondary]] == true)) {
			motor[PRT_feedLower] = 127;
		} else if ((DRV_config[FeedLowerOut] != UNASSIGNED && vexRT[DRV_config[FeedLowerOut]] == true) ||
			(DRV_config[FeedLowerOutSecondary] != UNASSIGNED && vexRT[DRV_config[FeedLowerOutSecondary]] == true)) {
			motor[PRT_feedLower] = -127;
		} else {
			motor[PRT_feedLower] = 0;
		}

		if ((DRV_config[FeedUpperIn] != UNASSIGNED && vexRT[DRV_config[FeedUpperIn]] == true) ||
			(DRV_config[FeedUpperInSecondary] != UNASSIGNED && vexRT[DRV_config[FeedUpperInSecondary]] == true)) {
			motor[PRT_feedUpper] = 127;
		} else if ((DRV_config[FeedUpperOut] != UNASSIGNED && vexRT[DRV_config[FeedUpperOut]] == true) ||
			(DRV_config[FeedUpperOutSecondary] != UNASSIGNED && vexRT[DRV_config[FeedUpperOutSecondary]] == true)) {
			motor[PRT_feedUpper] = -127;
		} else if (DRV_config[FeedUpperInSmall] != UNASSIGNED && vexRT[DRV_config[FeedUpperInSmall]] == true) {
			motor[PRT_feedUpper] = 32;
		} else {
			motor[PRT_feedUpper] = 0;
		}

		// Gun Control
		if (DRV_controllerButtonsDown[GunIncrement] == true) {
			if (PID_enabled == true)
				//USR_targetRange += USR_RANGE_LARGE_INCREMENT;
				USR_targetSpeed += 125;
			else if (GUN_enabled == true)
				GUN_maxMotorPower += GUN_LARGE_INCREMENT;
			DRV_controllerButtonsDown[GunIncrement] = false;
		} else if (DRV_controllerButtonsDown[GunDecrement] == true) {
			if (PID_enabled == true)
				//USR_targetRange -= USR_RANGE_LARGE_INCREMENT;
				USR_targetSpeed -= 125;
			else if (GUN_enabled == true)
				GUN_maxMotorPower -= GUN_LARGE_INCREMENT;
			DRV_controllerButtonsDown[GunDecrement] = false;
		}

		if (DRV_controllerButtonsDown[GunSmallIncrement] == true) {
			if (PID_enabled == true)
				//USR_targetRange += USR_RANGE_SMALL_INCREMENT;
				USR_targetSpeed += 25;
			else if (GUN_enabled == true)
				GUN_maxMotorPower += GUN_SMALL_INCREMENT;
			DRV_controllerButtonsDown[GunSmallIncrement] = false;
		} else if (DRV_controllerButtonsDown[GunSmallDecrement] == true) {
			if (PID_enabled == true)
				//USR_targetRange -= USR_RANGE_SMALL_INCREMENT;
				USR_targetSpeed -= 25;
			else if (GUN_enabled == true)
				GUN_maxMotorPower -= GUN_SMALL_INCREMENT;
			DRV_controllerButtonsDown[GunSmallDecrement] = false;
		}

		if (DRV_controllerButtonsDown[GunResetTarget] == true) {
			if (PID_enabled == true)
				//USR_targetRange = USR_DEFAULT_RANGE;
				USR_targetSpeed = USR_DEFAULT_SPEED;
			else if (GUN_enabled == true)
				GUN_maxMotorPower = GUN_DEFAULT_POWER;
			DRV_controllerButtonsDown[GunResetTarget] = false;
		}

		if (PID_target[Left] != 0 || PID_target[Right] != 0) {
			/*float dist = TRJ_angularSpeedAtRange(USR_targetRange);
			if (dist != SNR_INVALID) {
				PID_target[Left] = dist;
				PID_target[Right] = dist;
			}*/
			PID_target[Left] = USR_targetSpeed;
			PID_target[Right] = USR_targetSpeed;
		}

		if (DRV_controllerButtonsDown[GunWarm] == true) {
			if (PID_enabled == true) {
				if (PID_target[Left] != 0 || PID_target[Right] != 0) {
					PID_target[Left] = 0;
					PID_target[Right] = 0;
					//USR_targetRange = USR_DEFAULT_RANGE;
					USR_targetSpeed = USR_DEFAULT_SPEED;
				} else {
					PID_target[Left] = TRJ_angularSpeedAtRange(USR_targetRange);
					PID_target[Right] = PID_target[Left];
				}
			} else if (GUN_enabled == true) {
				GUN_maxMotorPower = GUN_DEFAULT_POWER;
				GUN_warming = !GUN_warming;
			}
			DRV_controllerButtonsDown[GunWarm] = false;
		}

		/* Sonar Capture
		if (vexRT[DRV_config[SonarCapture]] == true) {
			float dist = SNR_distanceInches;
			if (TRJ_validRange(dist)) {
				USR_targetRange = dist;
				if (PID_target[Left] != 0 || PID_target[Right] != 0) {
					PID_target[Left] = TRJ_angularSpeedAtRange(USR_targetRange);
					PID_target[Right] = PID_target[Left];
				}
			} else if (USR_sonarId == -1) {
				USR_sonarId = LED_startBlinkTask(Warning, Solid);
			}
		} else if (USR_sonarId != -1) {
			if (LED_stopBlinkTask(USR_sonarId))
				USR_sonarId = -1;
		}*/

		// Wheel Control
		if (DRV_controllerButtonsDown[ToggleMirror] == true) {
			USR_reverseMultiplier *= -1; // Flip the multiplier's sign.
			DRV_controllerButtonsDown[ToggleMirror] = false; // Consume event.
		}

		if (DRV_config[OmniLeft] != UNASSIGNED && DRV_config[OmniRight] != UNASSIGNED) {
			// 2 channel (tank) drive for OMNI WHEELS enabled.
			if (USR_reverseMultiplier < 0 && ((DRV_trimChannel(OmniLeft) < 0 && DRV_trimChannel(OmniRight) > 0)
				|| (DRV_trimChannel(OmniLeft) > 0 && DRV_trimChannel(OmniRight) < 0))) {
				// User is trying to turn while reversed.
				motor[PRT_wheelsLeft] = DRV_trimChannel(OmniLeft);
				motor[PRT_wheelsRight] = DRV_trimChannel(OmniRight);
				/*motor[PRT_wheelFrontLeft]  = DRV_trimChannel(OmniLeft);
				motor[PRT_wheelBackLeft]   = DRV_trimChannel(OmniLeft);
				motor[PRT_wheelFrontRight] = DRV_trimChannel(OmniRight);
				motor[PRT_wheelBackRight]  = DRV_trimChannel(OmniRight);*/
			}
			motor[PRT_wheelsLeft] = DRV_trimChannel(OmniLeft) * USR_reverseMultiplier;
			motor[PRT_wheelsRight] = DRV_trimChannel(OmniRight) * USR_reverseMultiplier;
			/*motor[PRT_wheelFrontLeft]  = DRV_trimChannel(OmniLeft)  * USR_reverseMultiplier;
			motor[PRT_wheelBackLeft]   = DRV_trimChannel(OmniLeft)  * USR_reverseMultiplier;
			motor[PRT_wheelFrontRight] = DRV_trimChannel(OmniRight) * USR_reverseMultiplier;
			motor[PRT_wheelBackRight]  = DRV_trimChannel(OmniRight) * USR_reverseMultiplier;*/
		} else if (DRV_config[OmniForward] != UNASSIGNED && DRV_config[OmniRotate] != UNASSIGNED) {
			// 2 channel (smart) drive for OMNI WHEELS enabled.
			if ((DRV_config[OmniMirrorForward] != UNASSIGNED && DRV_config[OmniMirrorRotate] != UNASSIGNED)
				&& (DRV_trimChannel(OmniForward) != 0 || DRV_trimChannel(OmniRotate) != 0)) {
				// To prevent setting motors to 0
				motor[PRT_wheelsLeft]   = (DRV_trimChannel(OmniForward) + DRV_trimChannel(OmniRotate)) * USR_reverseMultiplier;
				motor[PRT_wheelsRight] = (DRV_trimChannel(OmniForward) - DRV_trimChannel(OmniRotate)) * USR_reverseMultiplier;
				/*motor[PRT_wheelFrontLeft]  = (DRV_trimChannel(OmniForward) + DRV_trimChannel(OmniRotate)) * USR_reverseMultiplier;
				motor[PRT_wheelBackLeft]   = (DRV_trimChannel(OmniForward) + DRV_trimChannel(OmniRotate)) * USR_reverseMultiplier;
				motor[PRT_wheelFrontRight] = (DRV_trimChannel(OmniForward) - DRV_trimChannel(OmniRotate)) * USR_reverseMultiplier;
				motor[PRT_wheelBackRight]  = (DRV_trimChannel(OmniForward) - DRV_trimChannel(OmniRotate)) * USR_reverseMultiplier;*/
			}
		} else if (DRV_config[MecanumRotate] == UNASSIGNED) {
			// 4 channel (tank) drive for MECANUM WHEELS enabled.
			/*int threshold = 20;
			int RX = DRV_trimChannel(MecanumRightStrafe);
			int RY = DRV_trimChannel(MecanumRightNormal);
			int LX = DRV_trimChannel(MecanumLeftStrafe);
			int LY = DRV_trimChannel(MecanumLeftNormal);

			if (abs(RX) < threshold) {
				// Straight
				motor[PRT_wheelFrontRight] = RY;
				motor[PRT_wheelBackRight] = RY;
			} else if (RX > threshold) {
				// Right
				motor[PRT_wheelFrontRight] = RX;
				motor[PRT_wheelBackRight] = -RX;
			} else if (RX < -threshold) {
				// Left
				motor[PRT_wheelFrontRight] = RX;
				motor[PRT_wheelBackRight] = -RX;
			} else {
				// Stop
				motor[PRT_wheelFrontRight] = 0;
				motor[PRT_wheelBackRight] = 0;
			}

			if (abs(LX) < threshold) {
				// Straight
				motor[PRT_wheelFrontLeft] = LY;
				motor[PRT_wheelBackLeft] = LY;
			} else if (LX > threshold) {
				// Right
				motor[PRT_wheelFrontLeft] = -LX;
				motor[PRT_wheelBackLeft] = LX;
			} else if (LX < -threshold) {
				// Left
				motor[PRT_wheelFrontLeft] = -LX;
				motor[PRT_wheelBackLeft] = LX;
			} else {
				// Stop
				motor[PRT_wheelFrontLeft] = 0;
				motor[PRT_wheelBackLeft] = 0;
			}
		} else if (DRV_config[MecanumRotate] != UNASSIGNED) {
			// 3 channel (smart) drive for MECANUM WHEELS enabled.
			motor[PRT_wheelFrontRight] 	= DRV_trimChannel(MecanumRightNormal)
				- DRV_trimChannel(MecanumRotate)
				+ DRV_trimChannel(MecanumRightStrafe);
   		motor[PRT_wheelBackRight] 	= DRV_trimChannel(MecanumRightNormal)
   			+ abs(DRV_trimChannel(MecanumRotate))
   			- DRV_trimChannel(MecanumRightStrafe);
    	motor[PRT_wheelFrontLeft] 	= DRV_trimChannel(MecanumLeftNormal)
    		+ DRV_trimChannel(MecanumRotate)
    		- DRV_trimChannel(MecanumLeftStrafe);
    	motor[PRT_wheelBackLeft] 		= DRV_trimChannel(MecanumLeftNormal)
    		+ abs(DRV_trimChannel(MecanumRotate))
    		+ DRV_trimChannel(MecanumLeftStrafe);*/
		}

		if (DRV_config[OmniMirrorForward] != UNASSIGNED && DRV_config[OmniMirrorRotate] != UNASSIGNED
			&& (DRV_trimChannel(OmniForward) == 0 && DRV_trimChannel(OmniRotate) == 0)) {
			// 2 channel (smart) MIRRORED drive for OMNI WHEELS enabled.
			motor[PRT_wheelsLeft]   = (DRV_trimChannel(OmniMirrorForward) + DRV_trimChannel(OmniMirrorRotate)) * -1;
			motor[PRT_wheelsRight] = (DRV_trimChannel(OmniMirrorForward) - DRV_trimChannel(OmniMirrorRotate)) * -1;
			/*motor[PRT_wheelFrontLeft]  = (DRV_trimChannel(OmniMirrorForward) + DRV_trimChannel(OmniMirrorRotate)) * -1;
			motor[PRT_wheelBackLeft]   = (DRV_trimChannel(OmniMirrorForward) + DRV_trimChannel(OmniMirrorRotate)) * -1;
			motor[PRT_wheelFrontRight] = (DRV_trimChannel(OmniMirrorForward) - DRV_trimChannel(OmniMirrorRotate)) * -1;
			motor[PRT_wheelBackRight]  = (DRV_trimChannel(OmniMirrorForward) - DRV_trimChannel(OmniMirrorRotate)) * -1;*/
		}

		// Ping
		// Flashes lights on cortex while ping button is down to indicate responsiveness.
		if (vexRT[DRV_config[Ping]] && USR_pingId == -1) {
			USR_pingId = LED_startBlinkTask(Info, Solid);
		} else if (USR_pingId != -1) {
			if (LED_stopBlinkTask(USR_pingId))
				USR_pingId = -1;
		}

		// Override controls
		if (vexRT[Btn5U] == true && vexRT[Btn5D] == true && vexRT[Btn6U] == true && vexRT[Btn6D] == true &&
				vexRT[Btn5UXmtr2] == true && vexRT[Btn5DXmtr2] == true && vexRT[Btn6UXmtr2] == true && vexRT[Btn6DXmtr2] == true &&
				DRV_currentController != 3 && DRV_controllerOverrideDown < 3) {
			// Reset all controls to default settings.
			DRV_currentController = 3;
			DRV_controllerOverrideDown = 3;
			DRV_setupConfig();
			if (DRV_controllerWarningLed != -1 && LED_stopBlinkTask(DRV_controllerWarningLed)) {
				DRV_controllerWarningLed = -1;
			}
		} else if (vexRT[Btn5U] == true && vexRT[Btn5D] == true && vexRT[Btn6U] == true && vexRT[Btn6D] == true && DRV_currentController != 1 && DRV_controllerOverrideDown < 1) {
			DRV_currentController = 1;
			DRV_controllerOverrideDown = 1;
			if (DRV_controllerWarningLed == -1) {
				DRV_controllerWarningLed = LED_startBlinkTask(Warning, Irregular);
			}
			for (int i = 0; i < sizeof(DRV_config) / sizeof(DRV_config[0]); i++) {
					// Switch all controls over to PRIMARY controller.
					DRV_config[i] = DRV_translateXmtr(DRV_config[i]);
			}
		} else if (vexRT[Btn5UXmtr2] == true && vexRT[Btn5DXmtr2] == true && vexRT[Btn6UXmtr2] == true &&
			vexRT[Btn6DXmtr2] == true && DRV_currentController != 2 && DRV_controllerOverrideDown < 2) {
			DRV_currentController = 2;
			DRV_controllerOverrideDown = 2;
			if (DRV_controllerWarningLed == -1) {
				DRV_controllerWarningLed = LED_startBlinkTask(Warning, Irregular);
			}
			for (int i = 0; i < sizeof(DRV_config) / sizeof(DRV_config[0]); i++) {
					// Switch all controls over to SECONDARY controller.
					DRV_config[i] = DRV_translateXmtr(DRV_config[i], true);
			}
		} else if (vexRT[Btn5U] == false && vexRT[Btn5D] == false && vexRT[Btn6U] == false && vexRT[Btn6D] == false &&
				vexRT[Btn5UXmtr2] == false && vexRT[Btn5DXmtr2] == false && vexRT[Btn6UXmtr2] == false && vexRT[Btn6DXmtr2] == false) {
			DRV_controllerOverrideDown = 0;
		}

		if (DRV_controllerButtonsDown[Override] == true) {
			// Switch PID to the old, timer based system.
			// Toggle the PID then reset motors.
			GUN_enabled = !GUN_enabled;
			PID_enabled = !PID_enabled;
			motor[PRT_gunLeft1] = 0;
			motor[PRT_gunLeft2] = 0;
			motor[PRT_gunLeft3] = 0;
			motor[PRT_gunRight1] = 0;
			motor[PRT_gunRight2] = 0;
			motor[PRT_gunRight3] = 0;

			// Indicate to the DriverControlModule that we have recieved the button press.
			DRV_controllerButtonsDown[Override] = false;
		}

		if (USR_FORCE_AUTON) {
			startTask(autonomous);
			break;
		}
	}
	writeDebugStreamLine("[Mode]: User Control mode disabled.");
}