MasterCode.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in7,    leftPot,        sensorPotentiometer)
#pragma config(Sensor, in8,    rightPot,       sensorPotentiometer)
#pragma config(Sensor, dgtl1,  catapultAll,    sensorDigitalOut)
#pragma config(Sensor, dgtl2,  hang,           sensorDigitalOut)
#pragma config(Sensor, dgtl3,  leftSwitch,     sensorTouch)
#pragma config(Sensor, dgtl4,  rightSwitch,    sensorTouch)
#pragma config(Sensor, dgtl5,  ,               sensorDigitalOut)
#pragma config(Sensor, dgtl7,  leftSonar,      sensorSONAR_cm)
#pragma config(Sensor, dgtl9,  frontSwitch,    sensorSONAR_cm)
#pragma config(Sensor, dgtl12, catapultHalf,   sensorDigitalOut)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           driveRight1,   tmotorVex393HighSpeed_HBridge, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port2,           intake,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,            ,             tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           liftRight1,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           liftRight2,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           liftLeft2,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           liftLeft1,     tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           driveRight2,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           driveLeft2,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          driveLeft1,    tmotorVex393HighSpeed_HBridge, openLoop, encoderPort, I2C_1)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

 #pragma platform(VEX)

/*
Code handles the following functions:

Autonomous control - based on potentiometer values

Driver Control -
-Driving - arcade style on two joysticks
-Lifting - Using right trigger
-Bucky ball intake - using left trigger
-Large ball intake - using right buttons
-PID control toggle - using top-right button
-Hanging assist control - using left buttons

*/

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

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

int x; //Forwards movement
int r; //Rotation (Clockwise is +)
bool pidToggle = false;
static bool pidLiftOnManual = true;
bool catOnManual = true;
bool hangOnManual = true;
bool largeToggle = false;
bool largeSafetyOn = false;


void pre_auton()
{

}


task autonomous()
{
	while(1)
	{
		if (leftButton())
		{
			if (autoSelector() == 1)
			{
				frontLeftStandard();
				break;
			}
			else if (autoSelector() == 2)
			{
				frontLeftDefense();
				break;
			}
			else if (autoSelector() == 3)
			{
				backLeft2Launch();
				break;
			}
		  else if (autoSelector() == 4)
		  {
				backLeft3Ball();
				break;
			}
			else if (autoSelector() == 5)
			{
				backLeftStash();
				break;
			}
		}

		else if (rightButton())
		{
			if (autoSelector() == 1)
			{
				frontRightStandard();
				break;
			}
			else if (autoSelector() == 2)
			{
				frontRightDefense();
				break;
			}
			else if (autoSelector() == 3)
			{
				backRight2Launch();
				break;
			}
		  else if (autoSelector() == 4)
		  {
				backRight3Ball();
				break;
			}
			else if (autoSelector() == 5)
			{
				backRightStash();
				break;
			}
		}
	}
}



task usercontrol()
{
	liftRequestedValue = (SensorValue(rightPot));
	pidLiftOn = true;
	StartTask(pidLiftHoldPosition);
	SensorValue(hang) = 1;

	while (true)
	{

		//Lift up button
  	if (vexRT[Btn6U] == 1)
  	{
  		pidLiftOn = false;
  		lift(127);
  	}

  	//Lift down button
  	else if (vexRT[Btn6D] == 1)
  	{
  		pidLiftOn = false;
  		lift(-127);
  	}

  	//Lift stop
  	else if (pidLiftOnManual && pidLiftOn == false && liftTask == false)
  	{
  		lift(0);
  		wait10Msec(25);
  		liftRequestedValue = liftHeight();
  		pidLiftOn = true;
		}
		else if (!pidLiftOnManual && liftTask == false)
		{
			pidLiftOn = false;
			lift(0);
		}

		//Set lift to stash height
		if (vexRT(Btn8R) == 1)
		{
			pidLiftOnManual = true;
			StartTask(liftHigh);
		}

		//Toggle PID Lift Control
		if (vexRT[Btn8U] == 1 && pidLiftOnManual == false && pidToggle == false)
		{
			pidLiftOnManual = true;
			pidToggle = true;
		}
		else if (vexRT[Btn8U] == 1 && pidLiftOnManual == true && pidToggle == false)
		{
			pidLiftOnManual = false;
			pidToggle = true;
		}
		else if (vexRT[Btn7U] == 1)
		{
			pidLiftOnManual = false;
			pidToggle = true;
		}
		else if (vexRT[Btn8U] == 0)
			pidToggle = false;


		//Toggle Catapult
		if (vexRT[Btn8D] == 1 && catOnManual == false && SensorValue(catapultAll) == false)
		{
			catOnManual = true;
			SensorValue(catapultAll) = true;
			SensorValue(catapultHalf) = true;
		}
		else if (vexRT[Btn8L] == 1 && catOnManual == false && SensorValue(catapultAll) == false)
		{
			catOnManual = true;
			SensorValue(catapultAll) = true;
		}
		else if ((vexRT[Btn8D] == 1 || vexRT[Btn8L] == 1) == 1 && catOnManual == false && SensorValue(catapultAll) == true)
		{
			catOnManual = true;
			SensorValue(catapultAll) = false;
			SensorValue(catapultHalf) = false;
		}
		else if (vexRT[Btn8D] == 0 && vexRT[Btn8L] == 0)
			catOnManual = false;


		//Toggle hang
		if (vexRT[Btn7D] == 1 && hangOnManual == false && SensorValue(hang) == false)
		{
			hangOnManual = true;
			SensorValue(hang) = true;
		}
		else if (vexRT[Btn7D] == 1 && hangOnManual == false && SensorValue(hang) == true)
		{
			hangOnManual = true;
			SensorValue(hang) = false;
		}
		else if (vexRT[Btn7D] == 0)
			hangOnManual = false;

		//BuckyBall Intake Out
		if (vexRT[Btn5U] == 1)
			buckyIntake(-127);

		//BuckyBall Intake In
		else if (vexRT[Btn5D] == 1)
			buckyIntake(127);

		//BuckyBall Intake Stop
		else
			buckyIntake(0);

		//Large Ball Intake Out
		if (vexRT[Btn8L] == 1)
			largeIntake(-127);

		//Large Ball Intake In
		else if (vexRT[Btn8D] == 1)
			largeIntake(127);

		//Large Ball Intake Stop
		else
			largeIntake(0);
		//Set control to x (forwards/backwards) and r (rotation)
 	  x = joyStickLimit(vexRT[Ch4]);
 	  r = joyStickLimit(vexRT[Ch1]);

 	  //Assign motor a value depending on wether the directions cancel out values.
 	  //One axis will control back and forth, the other rotation.
  	motor[driveLeft1] = (joyStickLimit(vexRT[Ch3] + vexRT[Ch1]));
 	 	motor[driveRight1] = (joyStickLimit(vexRT[Ch3] - vexRT[Ch1]));
 		motor[driveLeft2] = (joyStickLimit(vexRT[Ch3] + vexRT[Ch1]));
 		motor[driveRight2] = (joyStickLimit(vexRT[Ch3] - vexRT[Ch1]));
	}

}