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);
}
}