moreAdvancedOldCode.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    FlipperGyro,    sensorGyro)
#pragma config(Sensor, in2,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in3,    FlipperPot,     sensorPotentiometer)
#pragma config(Sensor, in4,    Auton_Pot,      sensorPotentiometer)
#pragma config(Sensor, in5,    Gyro,           sensorNone)
#pragma config(Sensor, dgtl1,  JawChanger,     sensorTouch)
#pragma config(Sensor, dgtl11, LED,            sensorLEDtoVCC)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_5,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_6,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           RightBaseBack, tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port2,           LeftArm,       tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_4, 1000)
#pragma config(Motor,  port3,           RightArm,      tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port4,           LeftBaseFront, tmotorVex393, openLoop)
#pragma config(Motor,  port5,           TheJaw,        tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           FlipperMotorLeft, tmotorVex269, openLoop, reversed, encoder, encoderPort, I2C_5, 1000)
#pragma config(Motor,  port7,           FlipperMotorRight, tmotorVex269, openLoop, encoder, encoderPort, I2C_6, 1000)
#pragma config(Motor,  port8,           LeftBaseBack,  tmotorVex393, openLoop)
#pragma config(Motor,  port9,           Convayer,      tmotorVex393, openLoop)
#pragma config(Motor,  port10,+
RightBaseFront, tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_1, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//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!

// Pot Values - for the ARMS
#define Trough 			2222
#define HighGoal  	2770 //2780
#define HighLimit 	3000
#define LowLimit		1080
#define ArmsFloor 	1060
#define ArmsHigh	 	2780 //2720
// Arm Commands
bool goup = true;
bool godown = true;

// Range values for the autonomous dial
#define Range0 	5000
#define Range1	2000
#define Range2	1000
#define Range3	0

// Flipper values
#define FlipperHold 				3500
#define FlipperToFloor 			3260 //2750 //2600//2500//2600//2560
#define FlipperScore 				830

task BatmanHold()
{
	while(1)
	{
		//        400 = Tempo
		//          5 = Default octave
		//     Eighth = Default note length
		//        10% = Break between notes
		//
		PlayTone(  587,   14); wait1Msec( 150);  // Note(D)
		PlayTone(  587,   14); wait1Msec( 150);  // Note(D)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  523,   14); wait1Msec( 150);  // Note(C)
		PlayTone(  523,   14); wait1Msec( 150);  // Note(C)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  587,   14); wait1Msec( 150);  // Note(D)
		PlayTone(  587,   14); wait1Msec( 150);  // Note(D)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  523,   14); wait1Msec( 150);  // Note(C)
		PlayTone(  523,   14); wait1Msec( 150);  // Note(C)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  587,   14); wait1Msec( 150);  // Note(D)
		PlayTone(  622,   14); wait1Msec( 150);  // Note(D#)
		PlayTone(  523,   14); wait1Msec( 150);  // Note(C)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  523,   14); wait1Msec( 150);  // Note(C)
		PlayTone(  523,   14); wait1Msec( 150);  // Note(C)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  554,   14); wait1Msec( 150);  // Note(C#)
		PlayTone(  698,   14); wait1Msec( 150);  // Note(F)
		PlayTone(    0,   14); wait1Msec( 150);  // Note(Rest)
		PlayTone(  698,   27); wait1Msec( 300);  // Note(F, Duration(Quarter))
	}
}


task MulBat()
{
	while(1)
	{
		if(SensorValue(ArmPot) >= HighGoal)
		{
			//        180 = Tempo
			//          5 = Default octave
			//    Quarter = Default note length
			//        10% = Break between notes
			//
			PlayTone(  587,   15); wait1Msec( 167);  // Note(D, Duration(Eighth))
			PlayTone(  587,   15); wait1Msec( 167);  // Note(D, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  587,   15); wait1Msec( 167);  // Note(D, Duration(Eighth))
			PlayTone(  587,   15); wait1Msec( 167);  // Note(D, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  587,   30); wait1Msec( 333);  // Note(D)
			PlayTone(    0,   15); wait1Msec( 167);  // Note(Rest, Duration(Eighth))
			PlayTone(  622,   15); wait1Msec( 167);  // Note(D#, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  523,   15); wait1Msec( 167);  // Note(C, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  554,   15); wait1Msec( 167);  // Note(C#, Duration(Eighth))
			PlayTone(  698,   15); wait1Msec( 167);  // Note(F, Duration(Eighth))
			PlayTone(    0,   15); wait1Msec( 167);  // Note(Rest, Duration(Eighth))
			PlayTone(  698,   30); wait1Msec( 333);  // Note(F)
			return;
		}
	}
}

// LCD buttons
//const short leftButton = 1;
//const short centerButton = 2;
//const short rightButton = 4;

void WaitForLCD()
{

	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
	}
}

void FlipperGoDown()
{
	if (SensorValue [FlipperPot] > FlipperToFloor)
	{
		while (SensorValue [FlipperPot] > FlipperToFloor)
		{
			motor(FlipperMotorLeft) = -63;
			motor(FlipperMotorRight) = -63;
		}
		motor(FlipperMotorLeft) = 0;
		motor(FlipperMotorRight) = 0;
	}
}

void FlipperToTrough()
{
	// first reead the pot on flipper into a local variable
	// keep movimg the flliper down until it reaCHES  the "trough" pot setting
	// once it reaches the trough setting - quit moving the flipper
	while (SensorValue[FlipperPot] > FlipperScore)
	{
		motor(FlipperMotorRight) = -83;
		motor(FlipperMotorLeft) = -83;
	}
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}

task TASKflipperup()
{
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
	while(true)
	{
		if (SensorValue [FlipperPot] <  3050) //2650
		{
			motor(FlipperMotorLeft) = 60;
			motor(FlipperMotorRight) = 60;
			wait1Msec(10);
			motor(FlipperMotorLeft) = 10;
			motor(FlipperMotorRight) = 10;
		}
		else if (SensorValue [FlipperPot] >  3000) // 2600
		{
			motor(FlipperMotorLeft) = -60;
			motor(FlipperMotorRight) = -60;
			wait1Msec(10);
			motor(FlipperMotorLeft) = 10;
			motor(FlipperMotorRight) = 10;
		}
		else
		{
			motor(FlipperMotorLeft) = 10;
			motor(FlipperMotorRight) = 10;
		}
	}
}

task TASKflippertofloorandhold
{
	// raising up from trough position scored
	while (SensorValue [FlipperPot] < FlipperToFloor)
	{
		motor(FlipperMotorLeft) = 127;
		motor(FlipperMotorRight) = 127;
	}
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
}

void ClearEncoders()
{
	nMotorEncoder[RightBaseBack] = 0;
	nMotorEncoder[RightBaseFront] = 0;
	nMotorEncoder[LeftBaseBack] = 0;
	nMotorEncoder[LeftBaseFront] = 0;
}

task TASKKeepFlipperInPlace()
{
	// first if we are too high then the flipper must go down
	while (SensorValue[FlipperPot] > 1000)
	{
		motor(FlipperMotorRight) = -50;
		motor(FlipperMotorLeft) = -50;
	}
	while(true)
	{
		motor(FlipperMotorRight) = 20;
		motor(FlipperMotorLeft) = 20;
		while (SensorValue[FlipperPot] > 1200)
		{
			// too high
			motor(FlipperMotorRight) = -10;
			motor(FlipperMotorLeft) = -10;
		}
		while(SensorValue[FlipperPot] < 900)
		{
			// too low
			motor(FlipperMotorRight) = 40;
			motor(FlipperMotorLeft) = 40;
		}
	}
}

void MoveHalfSpeed(bool bForwards, int iPosition)
{
	ClearEncoders();
	while (abs(nMotorEncoder(RightBaseBack)) < iPosition)
	{
		if (bForwards)
		{
			motor[RightBaseFront] = 60;
			motor[RightBaseBack] = 60;
			motor[LeftBaseFront] = 60;
			motor[LeftBaseBack] = 60;
		}
		else
		{
			motor[RightBaseFront] = -60;
			motor[RightBaseBack] = -60;
			motor[LeftBaseFront] = -60;
			motor[LeftBaseBack] = -60;
		}
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void MoveFullSpeed(bool bForwards, int iPosition)
{
	ClearEncoders();
	while (abs(nMotorEncoder(RightBaseBack)) < iPosition)
	{
		if (bForwards)
		{
			motor[RightBaseFront] = 127;
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront] = 127;
			motor[LeftBaseBack] = 127;
		}
		else
		{
			motor[RightBaseFront] = -127;
			motor[RightBaseBack] = -127;
			motor[LeftBaseFront] = -127;
			motor[LeftBaseBack] = -127;
		}
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

#define ArmsMotorHold 10

bool ArmsHighNow = true;

void Arms(bool bUp, int iPosition, int iMotorPower)
{
	//Move arms to trough, floor, or high
	// are we moving up or down
	if (bUp)
	{
		// we are moving up
		while (SensorValue[ArmPot] < iPosition)
		{
			motor(RightArm) = iMotorPower;
			motor(LeftArm) = iMotorPower;
			motor(RightBaseBack) = vexRT(Ch2);
			motor(LeftBaseBack) = vexRT(Ch3);
		}
		// no movement - HOLD
		ArmsHighNow = true;
		motor(RightArm) = ArmsMotorHold;
		motor(LeftArm) = ArmsMotorHold;
	}
	else if (!bUp)
	{
		// we are moving down
		while (SensorValue[ArmPot] > iPosition)
		{
			motor(RightArm) = -iMotorPower;
			motor(LeftArm) = -iMotorPower;
		}
		// no movement - HOLD
		motor(RightArm) = 0;
		motor(LeftArm) = 0;
	}
}

#define ArmsTrough 2222//2260
task TASKarmstofloor()
{
	while (SensorValue[ArmPot] > ArmsFloor)
	{
		motor(RightArm) = -127;
		motor(LeftArm) = -127;
	}
	motor(RightArm) = 0;
	motor(LeftArm) = 0;
}

task YellowFlipperHold()
{
	while(1)
	{

		if(SensorValue(FlipperPot) < 2700)//2883
		{
			motor(FlipperMotorLeft) = 50;
			motor(FlipperMotorRight) = 50;
		}
		else if(SensorValue(FlipperPot) > 2800)//3000//3361
		{
			motor(FlipperMotorLeft) = -10;
			motor(FlipperMotorRight) = -10;
		}
		else if(SensorValue(FlipperPot) > 2700 && SensorValue(FlipperPot) < 2800)//3361
		{
			motor(FlipperMotorLeft) = 10;
			motor(FlipperMotorRight) = 10;
		}
	}
}

void flipperdown()
{
	motor(FlipperMotorLeft) = -63;
	motor(FlipperMotorRight) = -63;
	wait1Msec(700);
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}



////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////Start Autonomous Voids///////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void AutonomousScoreFive()
{
	flipperdown();

	//drive forward to five stack
	MoveFullSpeed(true, 900); //1100

	StartTask(TASKflipperup);

	while(SensorValue(ArmPot) < Trough)
	{
		motor(RightArm) = 127;
		motor(LeftArm) = 127;
	}
	motor(RightArm) = 10;
	motor(LeftArm) = 10;

	// drive forward
	MoveHalfSpeed(true, 400);//300//400//500

	StopTask(TASKflipperup);

	FlipperToTrough();
	wait1Msec(500);

	// drive back to starting tile
	MoveFullSpeed(false, 1200);

	StartTask(TASKflippertofloorandhold);
	StartTask(TASKarmstofloor);
	///////////////////////////
	// now pickup the yellow //
	///////////////////////////
	WaitForLCD();
	motor(FlipperMotorLeft) = -63;
	motor(FlipperMotorRight) = -63;
	wait1Msec(250);
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
	//drive forward to yellow bag
	MoveFullSpeed(true, 1600);

	// raise slightly and hold
	motor[FlipperMotorLeft] = 127;
	motor[FlipperMotorRight] = 127;
	MoveFullSpeed(true, 600);
	motor[FlipperMotorLeft] = 10;
	motor[FlipperMotorRight] = 10;

	// back up
	MoveFullSpeed(false, 2000);
	wait1Msec(10000);
}

void AutonomousScoreYellow()
{
	FlipperGoDown();
	//drive forward to yellow bag
	MoveFullSpeed(true, /*1500*/1600);
	// raise trough slightly and hold
	motor[FlipperMotorLeft] = 127;
	motor[FlipperMotorRight] = 127;
	MoveFullSpeed(true, 600);
	motor[FlipperMotorLeft] = 10;
	motor[FlipperMotorRight] = 10;
	MoveFullSpeed(false, 1400);//1250//900
	// lowering flipper so bags dont fall out
	motor[FlipperMotorLeft] = -80;
	motor[FlipperMotorRight] = -80;
	wait1Msec(250);
	motor[FlipperMotorLeft] = 10;
	motor[FlipperMotorRight] = 10;
	// raise arms up to trough
	StartTask(YellowFlipperHold);
	Arms(true, ArmsTrough, 127);
	MoveFullSpeed(true, 350);//230
	StopTask(YellowFlipperHold);
	FlipperToTrough();
	motor(LeftArm) = 50;
	motor(RightArm) = 50;
	wait1Msec(500);
	MoveFullSpeed(false, 10);
	motor(RightArm) = 0;
	motor(LeftArm) = 0;
	// drive back to starting tile
	MoveFullSpeed(false, 1200);
}

void Fuse()
{
	bool go = true;
	//part 1 drive forward

	motor(FlipperMotorLeft) = -63;
	motor(FlipperMotorRight) = -63;
	wait1Msec(800);
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;


	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(5000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////// END  Autonomous Voids///////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//Wait for Press--------------------------------------------------
void waitForPress()
{
	while(nLCDButtons == 0){}
	wait1Msec(5);
}
//----------------------------------------------------------------

//Wait for Release------------------------------------------------
void waitForRelease()
{
	while(nLCDButtons != 0){}
	wait1Msec(5);
}
//----------------------------------------------------------------

bool ScoreHighHoldButton = false;
// global variable to hold the gyro value
void CheckScoreHigh()
{
	if(vexRT(Btn7UXmtr2) == true)
	{
		if (ScoreHighHoldButton == true)
		{
			SensorValue(LED) = 0; // LED OFF
			StopTask(BatmanHold);
			StopTask(TASKKeepFlipperInPlace);
			wait1Msec(500);
			// turn off the hold
			motor(FlipperMotorLeft) = 0;
			motor(FlipperMotorRight) = 0;
			motor(RightArm) = 0;
			motor(LeftArm) = 0;
			ScoreHighHoldButton = false;
		}
		else
		{
			SensorValue(LED) = 1;  // LED ON
			wait1Msec(500);
			StartTask(BatmanHold);
			ScoreHighHoldButton = true;
			//hold the flipper with a value that will hold five bags
			StartTask(TASKKeepFlipperInPlace);
			Arms(true, ArmsHigh, 127);
			// hold arms
			motor(RightArm) = 20;
			motor(LeftArm) = 20;
		}
	}
	else
	{
		// if the button is not pressed, do nothing (until the button is pressed again)
	}
}



///////////////////////////////////////Start User Control Voids/////////////////////////////////////////////////////////
// The controls for the user control base.  It has PID in it (nMotorPIDSpeedCtrl[motorX] = mtrSpeedReg in #pragma config)
// as now doesn't have a ramp motor.
void base()
{
	if(vexRT(Btn6U) == 1)
	{
		motor(LeftBaseBack) = vexRT(Ch3);
		motor(LeftBaseFront) = vexRT(Ch3);
		motor(RightBaseBack) = vexRT(Ch2);
		motor(RightBaseFront) = vexRT(Ch2);
	}
	else
	{
		motor(LeftBaseBack) = vexRT(Ch3)/2;
		motor(LeftBaseFront) = vexRT(Ch3)/2;
		motor(RightBaseBack) = vexRT(Ch2)/2;
		motor(RightBaseFront) = vexRT(Ch2)/2;
	}
}

void UArms()
{
	if(vexRT(Btn8UXmtr2) == 1)
	{
		goup = true;
		while(goup == true)
		{
			motor(RightArm) = 127;
			motor(LeftArm) = 127;
			if(SensorValue(ArmPot) ==  HighGoal || SensorValue(ArmPot) >= HighLimit)
			{
				goup = false;
				motor(RightArm) = 0;
				motor(LeftArm) = 0;
			}
		}
	}

	motor[RightArm] = vexRT(Ch2Xmtr2)+5; // Lifter Motor 1
	motor[LeftArm] = vexRT(Ch2Xmtr2)+5; // Lifter Motor 2

	if(vexRT(Btn8DXmtr2) == 1)
	{
		godown = true;
		while(godown == true)
		{
			motor(RightArm) = -127;
			motor(LeftArm) = -127;
			if(SensorValue(ArmPot) ==  HighGoal || SensorValue(ArmPot) <= LowLimit)
			{
				godown = false;
				motor(RightArm) = 0;
				motor(LeftArm) = 0;
			}
		}
	}
}

task ConvayerTask()
{
	while(1)
	{
		if(vexRT(Btn6UXmtr2) == 1)
		{
			motor(Convayer) = 127;
		}
		if(vexRT(Btn6DXmtr2) == 1)
		{
			motor(Convayer) = -127;
		}
		if(vexRT(Btn6DXmtr2) == 0 && vexRT(Btn6UXmtr2) == 0)
		{
			motor(Convayer) = 0;
		}
	}
}

void Flipper()
{
	motor[FlipperMotorLeft] = vexRT(Ch3Xmtr2);
	motor[FlipperMotorRight] = vexRT(Ch3Xmtr2);
}

void HighArmButton()
{
	if(vexRT(Btn7U) == 0)
	{
		Arms(true, ArmsHigh, 127);
	}
}

int FlipperGyroValue = 1;
int FlipperMotorHoldValue = 0;

task FlipperGyroHold()
{
	// start holding the flipper in place - enter an infinite loop (until stop task is called, the code in here will continue to operate)
	while(true)
	{
		bool bMoveDown = false;
		bool bMoveUp = false;
		// do whatever you need to do to keep the flipper near the value

		if (SensorValue(FlipperGyro) > (FlipperGyroValue + 70))
		{
			// higher then the allowed threshold = start moving down
			bMoveUp = false;
			bMoveDown = true;
		}
		else if (SensorValue(FlipperGyro) < (FlipperGyroValue - 40))
		{
			// lower then the allowed threshold = start moving up
			bMoveUp = true;
			bMoveDown = false;
		}

		if(bMoveUp)
		{
			FlipperMotorHoldValue -= 1;
			motor(FlipperMotorRight) = -40;// FlipperMotorHoldValue;
			motor(FlipperMotorLeft) = -40; //FlipperMotorHoldValue;
			//wait1Msec(10);
		}
		else if(bMoveDown)
		{
			FlipperMotorHoldValue += 1;
			motor(FlipperMotorRight) = 40;// FlipperMotorHoldValue;
			motor(FlipperMotorLeft) = 40;//FlipperMotorHoldValue;
			//wait1Msec(10);
		}
		else
		{
			motor(FlipperMotorLeft) = 10;
			motor(FlipperMotorRight) = 10;
		}
		//wait1Msec(10);
	}
}


// global variable to check the status of the flipper hold button press
bool FlipperHoldButton = false;
// global variable to hold the gyro value

// add this as one of the functions that gets called in the infinite loop in the user control section of the code
void CheckFlipperHold()
{
	if(vexRT(Btn5UXmtr2) == true)
	{
		if (FlipperHoldButton == true)
		{
			SensorValue(LED) = 0; // LED OFF
			wait1Msec(500);
			// turn off the hold
			StopTask(BatmanHold);
			StopTask(FlipperGyroHold);
			FlipperHoldButton = false;
		}
		else
		{
			wait1Msec(500);
			// turn on the hold
			// get the current value of the gyro so we know where to hold the flipper
			while(SensorValue(FlipperPot) < FlipperHold)
			{
				motor(FlipperMotorLeft) = 70;
				motor(FlipperMotorRight) = 70;
			}
			FlipperGyroValue = SensorValue(FlipperGyro);
			SensorValue(LED) = 1; // LED ON
			StartTask(BatmanHold);
			StartTask(FlipperGyroHold);
			FlipperHoldButton = true;
		}
	}
	else
	{
		// if the button is not pressed, do nothing (until the button is pressed again)
	}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
//////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	bLCDBacklight = true;
	// 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 = true;
	SensorType[in1] = sensorNone;
	wait1Msec(1000);
	//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
	SensorType[in1] = sensorGyro;
	wait1Msec(2000);
	if(SensorValue(Auton_Pot) > Range1 && SensorValue(Auton_Pot) < Range0)
	{
		displayLCDCenteredString(0, "AutoScoreFive");
	}
	if(SensorValue(Auton_Pot) > Range2 && SensorValue(Auton_Pot) < Range1)
	{
		displayLCDCenteredString(0, "AutoScoreFive");

	}
	if(SensorValue(Auton_Pot) > Range3 && SensorValue(Auton_Pot) < Range2)
	{
		displayLCDCenteredString(0, "Fuse");

	}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	bLCDBacklight = true; // have LCD backlight on
	clearLCDLine(0);                                            // Clear line 1 (0) of the LCD
	clearLCDLine(1);                                            // Clear line 2 (1) of the LCD
	//Declare count variable to keep track of our choice
	int count = 0;

	//------------- Beginning of User Interface Code ---------------
	//Clear LCD
	clearLCDLine(0);
	clearLCDLine(1);
	while(1)
	{
		//Loop while center button is not pressed
		//Switch case that allows the user to choose from 4 different options
		switch(count){
		case 0:
			//Display first choice
			ClearEncoders();
			if(SensorValue(Auton_Pot) > Range1 && SensorValue(Auton_Pot) < Range0)
			{
				ClearEncoders();
				displayLCDCenteredString(0, "ScoreFive");
				AutonomousScoreFive();
				wait10Msec(5000);
			}
			if(SensorValue(Auton_Pot) > Range2 && SensorValue(Auton_Pot) < Range1)
			{
				ClearEncoders();
				displayLCDCenteredString(0, "ScoreYellow");
				AutonomousScoreYellow();
				wait10Msec(5000);
			}
			if(SensorValue(Auton_Pot) > Range3 && SensorValue(Auton_Pot) < Range2)
			{
				ClearEncoders();
				displayLCDCenteredString(0, "Fuse");
				Fuse();
				wait10Msec(5000);
			}
		}
	}
}


/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	StartTask(MulBat);
	StartTask(ConvayerTask);
	bLCDBacklight = true; // have LCD backlight on
	string mainBattery, backupBattery;
	//Clear the encoders associated with the left and right motors
	nMotorEncoder[RightBaseBack] = 0;
	nMotorEncoder[LeftBaseBack] = 0;
	nMotorEncoder[RightBaseFront] = 0;
	nMotorEncoder[LeftBaseFront] = 0;

	// Run Forever
	while(1)
	{

		// Displays the battery level on the LCD
		clearLCDLine(0);                                            // Clear line 1 (0) of the LCD
		clearLCDLine(1);                                            // Clear line 2 (1) of the LCD

		//Display the Primary Robot battery voltage
		displayLCDString(0, 0, "Primary: ");
		sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
		displayNextLCDString(mainBattery);

		//Display the Backup battery voltage
		displayLCDString(1, 0, "Backup: ");
		sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V');    //Build the value to be displayed
		displayNextLCDString(backupBattery);

		//Short delay for the LCD refresh rate
		wait1Msec(100);
		// Run the base program
		base();
		//DriverFlipper();
		if (ScoreHighHoldButton == false)
		{
			// run the arms program
			UArms();
		}
		// run the flipper program
		CheckScoreHigh();
		if (ScoreHighHoldButton == false || FlipperHoldButton == false )
		{
			// run Flipper program
			Flipper();
		}
		CheckFlipperHold();
	}
}
// end of code!!!!!!!!!!
//chupa chupa ;)
//lol what?
//UpUpDownDownLeftRightLeftRightBAStart