task autonomous()
{
while(true){
if(SensorValue[waitingButtonRed] == 1)
{
if(SensorValue[AutonSelect] < ALEX){
Alex();
}
else if(SensorValue[AutonSelect] < UDIT){
skills();
}
else if(SensorValue[AutonSelect] < DEVANSH){
redDevansh();
}
else{}
break;
}
else if(SensorValue[waitingButtonBlue] == 1)
{
if(SensorValue[AutonSelect] < ALEX)
Alex();
else if(SensorValue[AutonSelect] < UDIT)
skills();
else if(SensorValue[AutonSelect] < DEVANSH)
blueDevansh();
else{}
break;
}
}
}
/*********************************************************************************************************************************
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////// Competition PROGRAM HERE ///////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
**********************************************************************************************************************************/
task autonomous()
{
while(true)
{
if(SensorValue[redWaitingButton])
{
if(SensorValue[AutonSelect] < ALEX)
Alex();
else if(SensorValue[AutonSelect] < UDIT)
redUdit();
else if(SensorValue[AutonSelect] < DEVANSH)
redDevansh();
else{}
break;
} // end if
else if(SensorValue[blueWaitingButton])
{
if(SensorValue[AutonSelect] < ALEX)
Alex();
else if(SensorValue[AutonSelect] < UDIT)
blueUdit();
else if(SensorValue[AutonSelect] < DEVANSH)
blueDevansh();
else{}
break;
} // end else if
} // end while
} //end auton task
task autonomous()
{
deploy();
while(true)
{
if(SensorValue[waitingButtonRed] == 1)
{
if(SensorValue[AutonSelect] < ALEX)
{
Alex(); // 15 middle zone
}
else if(SensorValue[AutonSelect] < UDIT)
{
RedUdit(); // projected 30 pts + spare change (6-14)
}
else if(SensorValue[AutonSelect] < DEVANSH)
{
skills(); // safe 3-6
}
else{}
break;
}
else if(SensorValue[waitingButtonBlue] == 1)
{
if(SensorValue[AutonSelect] < ALEX)
Alex();
else if(SensorValue[AutonSelect] < UDIT)
blueUdit();
else if(SensorValue[AutonSelect] < DEVANSH)
skills();
else{}
break;
}
}
}
void autonTest()
{
Alex();
}
void skills()
{
Alex(); // 15 points
liftDown();
waitForButton();
wait10Msec(50);
while(SensorValue[waitingButtonRed] == 0 && SensorValue[waitingButtonBlue] == 0)
{
setRight(127); setLeft(127);
} // this works for crossing bump
resetValues(0); // I don't know why it doesn't reach here...
// end cross bump
waitForButton();
intake(1);
wait10Msec(10);
moveStraight(1, 0, 455); //picks up
wait10Msec(50);
moveStraight(-1, 0, 475);//comes back
// end pick up 3
waitForButton();
wait10Msec(50);
while(SensorValue[waitingButtonRed] == 0 && SensorValue[waitingButtonBlue] == 0)
{
setRight(127); setLeft(127);
}
resetValues(0);
// end cross bump
waitForButton();
moveStraight(1, 0, 1400); // 1600 is just before goal
lift(HIGH); // nearest 100
holdArmHigh();
moveStraight(1, 0, 350); // reaches goal
intake(-1);
wait1Msec(500); // outtake
moveStraight(-1, 0, 400); //move back away from goal...Apparently Safety is greater than move forward
liftDown();
moveStraight(-1, 0, 1500);
resetValues(0);
// end Alex 2.0
waitForButton();
wait10Msec(50);
while(SensorValue[waitingButtonRed] == 0 && SensorValue[waitingButtonBlue] == 0)
{
setRight(127); setLeft(127);
}
setRight(0); setLeft(0);
// end cross
waitForButton();
intake(1);
wait10Msec(100);
moveStraight(1, 0, 700);
resetValues(50);
// end intake big ball
moveStraight(-1, 0, 200);
softSpin(-1, 0, 200);
intake(-1);
resetValues(1200);
// end outtake
spin(1, 0, 200);
intake(1);
moveStraight(1, 0, 900);
intake(0);
wait10Msec(30);
moveStraight(-1, 0, 200);
softSpin(-1, 0, 200);
}
task usercontrol()
{
while(true)
{
// Instance Variables
int RightDrivePower, LeftDrivePower, LiftPower, IntakePower = 0;
/////////////////////////////
//*** DEAD ZONE CONTROL ***//
/////////////////////////////
int Channel3 = abs(vexRT(Ch3)) < control ? 0 : vexRT(Ch3); // deadzone setting - if abs(y) greater than 25 -> 0, else real value
int Channel1 = abs(vexRT(Ch1)) < control ? 0 : vexRT(Ch1); // deadzone setting - if abs(x) greater than 25 -> 0, else real value
///////////////////////////////
//****** DRIVE CONTROL ******//
///////////////////////////////
// Halo Drive
//Linear Scaling - Joseph has gotten used to this...
RightDrivePower = Channel3 - Channel1;
LeftDrivePower = Channel3 + Channel1;
//Soft Cubic Scaling - Joseph doesn't like the turning on this, but likes the extra precision
/*
RightDrivePower = cubicScaling(vexRT[Ch3]) - cubicScaling(vexRT[Ch1]);
LeftDrivePower = cubicScaling(vexRT[Ch3]) + cubicScaling(vexRT[Ch1]);
*/
// Manual Tank
/* //Soft Cubic Scaling
RightDrivePower = cubicScaling(vexRT[Ch2]);
LeftDrivePower = cubicScaling(vexRT[Ch3]);
*/
/* // Hard Cubic Scaling - Not Tested
RightDrivePower = hardCubicScaling(vexRT[Ch2]);
LeftDrivePower = hardCubicScaling(vexRT[Ch3]);
*/
///////////////////////////
//***** LIFT CONTROL ****//
///////////////////////////
/**************************
Liimits: Can't go lower than LOW
Arm Holding: Constant hold between PRE_HIGH and HIGH
**************************/
// Logic order -> if no input and already raised -> Constant
// If not -> Check Lowered Safety
// If not -> Manual Control
//PRE-CONDITION: Arm Holding is Autonomous,
//No user input, but user needs to take manual on demand
if((vexRT[Btn5U] == 0 && vexRT[Btn5D] == 0) && (SensorValue[RightArmAngle] >= (BUMP)))
LiftPower = hold;
else if(SensorValue[RightArmAngle] <= LOW) //LOW Safety Limit
LiftPower = vexRT[Btn5U]*127 - vexRT[Btn5D]*0; // can only go up now
else // Full Manual
LiftPower = vexRT[Btn5U]*127 - vexRT[Btn5D]*127;
if(vexRT[Btn8D] == 1) // left bottom button to set to barrier height, may need testing
{
while(SensorValue[RightArmAngle] != BARRIER)
{
if(SensorValue[RightArmAngle] < BARRIER)
LiftPower = 127;
break;
if(SensorValue[RightArmAngle] > BARRIER)
LiftPower = -127;
break;
}
}
/*
//prototype arm code
if (vexRT[Btn8U] == 1){
raiseArmBump = false;
raiseArmBarrier = false;
raiseArmHigh = true;
if (SensorValue[RightArmAngle] <= HIGH){
armDirection = 1;
}
else{
armDirection = -1;
}
}
else if (vexRT[Btn8R] == 1){
raiseArmBump = false;
raiseArmBarrier = true;
raiseArmHigh = false;
if (SensorValue[RightArmAngle] <= BARRIER){
armDirection = 1;
}
else{
armDirection = -1;
}
}
else if (vexRT[Btn8D] == 1){
raiseArmBump = true;
raiseArmBarrier = false;
raiseArmHigh = false;
if (SensorValue[RightArmAngle] <= BUMP){
armDirection = 1;
}
else{
armDirection = -1;
}
}
*/
//////////////////////////
//*** Intake Control ***//
//////////////////////////
IntakePower = vexRT[Btn6U]*127 - vexRT[Btn6D]*127;
/*************************
****** TESTING AREA ******
*************************/
/*
if(vexRT[Btn8L] == 1) // comment this before competition
{
autonTest();
}
*/
//Set motors to each individual powers...
motor[RightBWheel] = RightDrivePower; // port 4
motor[RightMWheel] = RightDrivePower; // port 3
motor[RightFWheel] = RightDrivePower; // po?vrt 2
//setRight(RightDrivePower);
motor[LeftFWheel] = LeftDrivePower; // port 9
motor[LeftMWheel] = LeftDrivePower; //port 8
motor[LeftBWheel] = LeftDrivePower; //port 7
//setLeft(LeftDrivePower);
motor[RightArm] = motor[LeftArm] = LiftPower;
motor[RightIntake] = motor[LeftIntake] = IntakePower;
if (vexRT[Btn8L] == 1){
Alex();
}
if (vexRT[Btn8R] == 1){
Alex2();
}
} // end while update loop
} // end task usercontrol