예제 #1
0
task autonomous()
{
	initializeTBH();
	FwVelocitySet(lFly, 74, 0.623);
	FwVelocitySet(rFly, 74, 0.623);
	wait1Msec(2810); //wait time 1st ball
	motor[intakeTop] = 125;
	wait1Msec(1220); //2nd ball wait
	moveIntake(621, 125);
	wait1Msec(1220); //3rd
	moveIntake(921, 125);
	wait1Msec(1320); //4th
	moveIntake(921, 125);
}
task usercontrol() {
while(1 == 1) {

/* Drivetrain */

//Right side of the robot is controlled by the right joystick, Y-axis
motor[frontRightMotor]	= vexRT[Ch2];
motor[backRightMotor] 	= vexRT[Ch2];

//Left side of the robot is controlled by the left joystick, Y-axis
motor[frontLeftMotor]   = vexRT[Ch3];
motor[backLeftMotor]    = vexRT[Ch3];



/* Button Control */

// Arm

if(vexRT[Btn5U]) {
moveArm(1); // Move arm up
}
else if(vexRT[Btn5D]) {
moveArm(-1); // Move arm down
}
// If neither buttons 5U or 5D are pressed
else {
moveArm(0);	//don't move arm

}
if(vexRT[Btn8L]==1)
{
motor[bottomLeftTower]= 64;
motor[bottomRightTower]= 64;
motor[topLeftTower] = 64;
motor[topRightTower]= 64;
}
if(vexRT[Btn8R]==1)
{
motor[bottomLeftTower] = -64;
motor[bottomRightTower]= -64;
motor[topLeftTower]= -64;
motor[topRightTower]= -64;
}


// Intake

if (vexRT[Btn6U]) {
moveIntake(1); //runs intake, pull
}
else if (vexRT[Btn6D]) {
moveIntake(-1); //runs intake, push
}
else {
moveIntake(0);
}

//*Pnumatics*//

if(vexRT[Btn7U]) {
//activate lock
SensorValue[pistonLock1] = 1;
SensorValue[pistonLock2] = 1;
}
else if(vexRT[Btn7D]) {
SensorValue[pistonLock1] = 0;
SensorValue[pistonLock2] = 0;
}

if(vexRT[Btn8U]==1)
{
if(SensorValue(potentiometer) < 1700){
	motor[topRightTower] = 127;
	motor[topLeftTower] = 127;
	motor[bottomLeftTower]= 127;
	motor[bottomRightTower] = 127;
}
else if(SensorValue(potentiometer) > 1700){
	motor[topRightTower] = 70;
	motor[topLeftTower] = 70;
	motor[bottomLeftTower]= 70;
	motor[bottomRightTower] = 70;
}
}
else if(vexRT[Btn8D]==1)
{
if(SensorValue(potentiometer) < 1700){
	motor[topRightTower] = -127;
	motor[topLeftTower] = -127;
	motor[bottomLeftTower]= -127;
	motor[bottomRightTower] = -127;
}
else if(SensorValue(potentiometer) > 1700){
	motor[topRightTower] = -70;
	motor[topLeftTower] = -70;
	motor[bottomLeftTower]= -70;
	motor[bottomRightTower] = -70;
}
}

//why is this commented out?
/*
else
{
motor[topRightTower] = 10;
motor[topLeftTower] = 10;
motor[bottomLeftTower]= 10;
motor[bottomRightTower] = 10;
}
*/

}
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// 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()
{


	// goal side Red
	if(programChoice == 1){

	// Move forward
	moveDrive(1, 250);

	//move back to pop out intake
	moveDrive(-1,250);

	//move to first position
	moveDrive(1,500);

	// Move arm up
	moveArm(1,750);

	// Move forward to second point
	moveDrive(1, 500);

	// Wait a secound so that big ball is out of the way
	wait1Msec(750);

	/*
	// Outtake preload
	moveIntake(-1);
	wait1Msec(2000);
	*/

	// Drive back to starting square
	moveDrive(-1, 1000);

	//stop robot for button press to reposition
	/* moveDrive(0,3000); */

	while (SensorValue[touchSensor] != 1){}

	//move robot forward at new angle
	moveDrive(1,1300);

	//dive back to starting square
	moveDrive(-1,1300);

	//stop robot wait for reposition
	 while (SensorValue[touchSensor] != 1) {}

	 //dive forward
	 moveDrive(1,1500);

	 //lift arm just a tig
	 moveArm(1,250);

	 //turn left 90 degrees
	 turnDrive(1,500);

	 //knock other colour bucky balls off the bump
	 moveDrive(1,250);
	}

	// goal side blue
	if(programChoice == 2){

	// Move forward
	moveDrive(1, 250);

	//move back to pop out intake
	moveDrive(-1,250);

	//move to first position
	moveDrive(1,500);

	// Move arm up
	moveArm(1,750);

	// Move forward to second point
	moveDrive(1, 500);

	// Wait a secound so that big ball is out of the way
	wait1Msec(750);

	// Outtake preload
	moveIntake(-1);
	wait1Msec(2000);

	// Drive back to starting square
	moveDrive(-1, 1000);

	//stop robot for button press to reposition
	/* moveDrive(0,3000); */

	while (SensorValue[touchSensor] != 1){}

		//move robot forward at new angle
	moveDrive(1,1300);

	//dive back to starting square
	moveDrive(-1,1300);

	//stop robot wait for reposition
	 while (SensorValue[touchSensor] != 1) {}

	 //dive forward
	 moveDrive(1,1500);

	 //lift arm just a tig
	 moveArm(1,250);

	 //turn right 90 degrees
	 turnDrive(-1,500);

	 //knock other colour bucky balls off the bump
	 moveDrive(1,250);
	}

	// Blue side hanging bar side of bump auto (auto 3)
	else if(programChoice == 3) {

	// Move forward
	moveDrive(1, 250);

	//move back to pop out intake
	moveDrive(-1,250);

	//move to first position
	moveDrive(1,500);

	//stop Robot and intake balls
	moveIntake(1);

	wait1Msec(750);

	//move robot back to starting square
	moveDrive(-1, 750);

	//Stop robot for reposition 90 degrees
	//moveDrive(0, 3000);
	while (SensorValue[touchSensor] != 1){}


	//Move robot in new direction
	moveDrive(1, 750);

	//robot Turn 90 degrees left
	turnDrive(1, 750);

	//Lift arm up a tig
	moveArm(1,500);

	//Move robot forward over the bump
	moveDrive(1,1500);

	//lift the arm all the way
	moveArm(1,750);

	//move forward alittle
	moveDrive(1,250);

	//outtake preload
	moveIntake(1);
	wait1Msec(2000);
	}

	//Red side hanging bar side of bump auto (auto 3)
	else if(programChoice ==4) {

	// Move forward
	moveDrive(1, 250);

	//move back to pop out intake
	moveDrive(-1,250);

	//move to first position
	moveDrive(1,500);

	//stop Robot and intake balls
	moveIntake(1);
	wait1Msec(750);

	//move drive back to original starting space
	moveDrive(-1, 750);

	//Stop robot for reposition 90 degrees
	//moveDrive(0, 3000);
	while (SensorValue[touchSensor] != 1){}

	//Move robot in new direction
	moveDrive(1, 750);

	//robot Turn 90 degrees Right
	turnDrive(-1, 750);

	//Lift arm up a tig
	moveArm(1,500);

	//Move robot forward over the bump
	moveDrive(1,1500);

	//move arm all the way up
	moveArm(1,750);

	//move move drive a tig
	moveDrive(1,250);

	//outtake preload
	moveIntake(1);
	wait1Msec(2000);
	}

	//skills challenge
	else if(programChoice == 3) {
		moveDrive(0);
	}
}