void rejectionRed()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int pot = analogRead (8);
int encoder1 = 1200; //drive under wall
int encoder2 = 136; // rotate 90 degrees
int encoder3 = 900; // backwards to the opponets goal
int encoder4 = 150; //rotate 90
int encoder5 = 200; // ass towards bridge
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack (encoder1); // drive backwards to under the bridge
stopIntake();
turnRight (encoder2); // turn ass to opponets goal
driveBack (encoder3); // drive to opponets goal
turnRight (encoder4); // ass to bridge
armUp(wallHeight);// arm up
driveBack(encoder5); // block their launch
stopAll ();
}
void rushBlue()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead(8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 630; //turn to goal + enclose it
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack(encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft(encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(85);
//driveBackDead(); // push them
//delay (300);
stopDrive();
delay(7000); // wait till auto end
driveBackDead(); // push them
delay(800);
stopDrive();
delay(100);
driveForwardDead(); // driveback towards goal
delay(300);
stopDrive();
intakeDead();
armUp(goalHeight); //arm up
scrapeLeft(encoder4); //encase the goal
driveForwardSlowDead();
delay(700);
stopDrive();
outtake(3000);
stopAll();
}
void worldsBlue ()
{
// variables
int armMin = 300;
int wallHeight = 100;
int goalHeight = 1550;
int dead1 = 1000;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead (8);
int encoder1 = 1500; //drive under wall
int encoder2 = 136; // rotate 90 degrees
int encoder3 = 900; // backwards to the opponets goal
int encoder4 = 400; //turn to goal + enclose it
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack (encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft (encoder2); // turn ass to opponets goal
driveBack (encoder3); // drive to opponets goal
driveBackDead(); // push them
delay (200);
stopDrive ();
delay (7000); // wait till auto end
driveBackDead ();// push them
delay (600);
stopDrive();
delay(100);
driveForwardDead(); // driveback towards goal
delay(400);
stopDrive ();
armUp(goalHeight); //arm up
scrapeLeft (encoder4); //encase the goal
outtake (300);
stopAll ();
}
void reverse15Red()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int pot = analogRead (8);
int encoder1 = 1200; //drive under wall
int encoder2 = 136; // rotate 90 degrees
int encoder3 = 1300; // backwards to the opponets goal
int encoder4 = 900; //turn to bridge
int encoder5 = 200; // knock their wallball
int encoder6 = 200; // turn to other ball
int encoder7 = 400; // knock their center ball
int encoder8 = 400; // positioning
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack (encoder1); // drive backwards to under the bridge
stopIntake();
turnRight (encoder2); // turn ass to opponets goal
driveBack (encoder3); // drive to opponets goal
driveBackDead(); // 90 deg align
delay(400);
stopDrive ();
armUp(wallHeight);// arm up
scrapeLeft (encoder4); // turn to wall ball
driveForward (encoder5); // knock off wall ball
scrapeLeftBack(encoder6); // turn to middle ball
driveForward(encoder7); // knock off middle ball
driveBack (encoder8); // positioning
stopAll ();
}
void kakitRed ()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int dead1 = 1000;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead (8);
int encoder1 = 1000;
int encoder2 = 150;
int encoder3 = 2000;
int encoder4 = 75;
int encoder5 = 75;
int encoder6 = 75;
int encoder7 = 75;
int encoder8 = 75;
int encoder9 = 75;
// begin routine
driveForwardDead (); //ram big balls
intakeDead ();
delay (3000);
stopIntake();
driveBackDead (4000); // wall align to 90 deg
driveForward (encoder1);
turnRight (encoder2); // turn towards buckys
intakeDead ();
//line follow forward (encoder 4)
driveBackSlowDead (); // allign the bump
driveBackDead(); // over the bump
driveForwardSlowDead(); // alighn to bump
driveBackSlow(encoder4);
turnRight (encoder5);
driveBackSlow (encoder6) ; // go under the bridge
armUpDead (); // break the bridge
delay(500);
armDown (armMin);
driveBack (encoder7);
turnRight (encoder8);
armUp (goalHeight);
//line follow (encoder9);
outtake(8000); // score all three balls in the goal
stopAll ();
}
void rejectionBlueSkills()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead(8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 270; //turn to center of large balls
int encoder5 = 200; //hit 1st blue ball off
int encoder55 = 300; // back a bit
int encoder6 = 450; //hit 2nd ball off
int encoder7 = 450; // drive back past line
int encoder8 = 160; // turn to line
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack(encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft(encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(85);
armUp(wallHeight); // arm up
turnRight(encoder4); // turn towards center of large balls
driveForwardSlowDead(); // go between large balls
delay(1200);
stopDrive();
delay(300);
turnLeft(encoder5); //hit center ball
driveBackSlow(encoder55);
turnRight(encoder6); // hit far ball
driveBackSlow(encoder7); // back up past line
turnRight(encoder8); // tun to line
armUp(goalHeight);
findLineLeft();
//followLine(300);
driveForwardDead(); // drive to goal
delay(700);
stopDrive();
outtake(3000);
stopAll();
delay(60000);
}
void kakitRed()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1650;
int pot = analogRead(8);
int encoder00 = 250; // back up abit to row
int encoder0 = 950; // turn 360 degrees, knock off buckys, face line
int encoder1 = 162; // rotate towards 2 buckys
int encoder2 = 150; // drive towards buckys
int encoder3 = 400; // back up to bump
int encoder4 = 200; // back up towards bridge
int encoder5 = 360; // rotate 180 degrees to the large ball
int encoder6 = 900; // go under bridge and knock out large ball
int encoder7 = 90; // turn to goal
int encoder8 = 200; // drive to goal
int encoder9 = 75;
// begin routine
intake(300);
armDownTrim();
driveForwardDead(); //ram big balls
delay(1500);
stopDrive();
delay(500);
driveBack(encoder00); // back up to row abit
intakeDead();
turnLeft(encoder0); // turn 360 degrees
driveBackDead(); // drive back + wall align
delay(1300);
stopDrive();
delay(500);
turnRight(encoder1); // turn to two buckys
intakeDead();
followLine(300); // make sure ur straight
driveForwardSlowDead(); // drive towards buckys
delay(500);
stopDrive();
delay(600);
driveForwardDead(); //get the 2nd ball
delay(200);
stopDrive();
delay(600);
stopIntake();
driveBack(encoder3); // back up to bump
armUpDead(); // armup
delay(50);
stopArm();
stopIntake();
driveBackSlowDead(); // allign the bump
delay(300);
stopDrive();
delay(500);
driveBackDead(); // over the bump
delay(1000);
stopDrive();
delay(500);
driveForwardSlowDead(); // alighn to bump
delay(800);
stopDrive();
delay(500);
driveBackSlow(encoder4); // back up towards bridge
turnLeft(encoder5); // rotate 180 degrees to the large ball
armDown(armMin);
armDownTrim();
driveForward(encoder6); // go under the bridge + knock large ball
armUp(goalHeight);
turnRight(encoder7); // turn to goal
findLineRight();
followLine(300);
driveForwardSlowDead(); // drive to goal
delay(700);
stopDrive();
outtake(7000); // outtake 3
stopAll();
}
void rejectionBlue()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead(8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 100; //turn to goal + enclose it
int encoder5 = 450; //hit 1st blue ball off
int encoder6 = 350; // drive back
int encoder7 = 175; // turn to 2nd blue ball
int encoder8 = 550; // hit 2nd ball off
int encoder9 = 250; // drive back to position
int encoder10 = 370; // rotate to line
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack(encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft(encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(85);
armUp(wallHeight); // arm up
turnRight(encoder4); // turn towards center blue ball
driveForward(encoder5); // hit 1st ball off
delay(700);
driveBack(encoder6); // back
delay(700);
turnRight(encoder7); // turn towards 2nd blue ball
delay(700);
driveForward(encoder8); // hit off 2nd blue ball
delay(700);
intakeDead();
driveBack(encoder9); // position
delay(700);
turnLeft(encoder10); // encase opponet goal
delay(700);
armUp(goalHeight);
findLineRight();
//followLine(300);
driveForwardDead(); // drive to goal
delay(700);
stopDrive();
outtake(3000);
stopAll();
delay(60000);
}
void classic15Blue()
{
int armMin = 290;
int wallHeight = 1000;
int goalHeight = 1700;
int dead1 = 1200;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead(8);
//encoder values
int encoder1 = 900; //drive to goal
int encoder2 = 325; //keep going towards goal
int encoder3 = 0; //drive slow, adjust to 90 degrees
int encoder4 = 325; //back up
int encoder5 = 1350; //back up across the barrier again
int encoder6 = 80; // turn towards center large ball
int encoder7 = 600; // hit 1st ball off the barrier
int encoder8 = 350; // drive back
int encoder9 = 200; // turn towards other large ball
int encoder10 = 400; // hit 2nd ball off the barrier
int encoder11 = 300; // drive back to square
int encoder12 = 290; // turn to barf
int encoder13 = 800; // drive to barf + pickup
int encoder14 = 400;
int encoder15 = 700;
// begin routine
intakeDead();
delay(1000);
stopIntake();
// driveforward (some curve)////////////////////////////////////////////////////
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < encoder1)
{
motorSet(MOTOR_DRIVE_RIGHT_BACK, 110); // slight curve cuz friction
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 110);
motorSet(MOTOR_ARM_LEFT_BACK, 127);
motorSet(MOTOR_ARM_LEFT_FRONT, 127);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
///////////////////////////////////////////////////////////////////////////////
//driveForward(encoder1); // drive to goal
armUp(goalHeight); //raise arm after cross barrier
driveForwardSlow(encoder2); //keep going towards goal
driveForwardSlowDead(); //drive slow, adjust to 90 degrees
delay(1000);
outtake(1700); // outtake
driveBack(encoder4); // back up
delay(300);
armDown(armMin); // lower arm
driveBack(encoder5); //back up across the barrier again
delay(300);
turnLeft(encoder6); // turn towards center large ball
armUp(wallHeight); // arm up to wall height
driveForward(encoder7); // hit it off the barrier
delay(500);
driveBack(encoder8); // drive back
delay(300);
turnRight(encoder9); // turn towards other large ball
driveForward(encoder10); // hit 2nd ball off the barrier
delay(500);
driveBack(encoder11); // drive back to square
delay(600);
turnLeft(encoder12); // turn to barf
delay(300);
armDown(armMin);
intakeDead(); // pick up barf
driveForward(encoder13); // drive towards barf + intake it
delay(500);
//end of routine
stopAll();
delay(60000); ///////////////////////////////////////////////////////////////////////////////////
}
task autonomous()/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
{
int programmingSkills=0;
int match=0;
int red=0;
int blue=0;
int cube=0;
int skyrise=0;
SensorValue[leftLifttEncoder] = 0;
SensorValue[rightLiftEncoder] = 0;
if(SensorValue[autoPot2] > 1000) // competition or match
{
programmingSkills=1;
}
else
match=1;
if(SensorValue[autoPot] < 2500) //red or blue
{
red=1;
}
else
blue=1;
if(SensorValue[autoPot3] > 2000) // skyrise or cubes
{
skyrise=1;
}
else
cube=1;
// DriveLoop(-1, 800, 0.11, 0.000000, 0.001);
// DriveLoop( 1, 800, 0.11, 0.000000, 0.001);
if(match==1 && skyrise==1 && red==1)//------------------RED SKYRISE-------
{
SensorValue[latch] = 1;
armDown(0, 90);
wait1Msec(300);
armUp(130, 90);
autoFeed(300, -127);
autoFeed(1,0);
wait1Msec(500);
armDown(105, 90);
autoGrab(1);
armUp(300, 90);
liftMotors(-18);
DriveLoop(-1,-1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(0, 90);
autoGrab(0); //drop one
armUp(160, 90);
liftMotors(-10);
DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(105, 90);
autoGrab(1);
armUp(200, 90);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(220, 90);
autoGrab(0); //drop two
armUp(290, 90);
liftMotors(-15);
DriveLoop(1,1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(105, 90);
autoGrab(1);
armUp(390, 90);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(320, 90);
autoGrab(0); //drop three
armUp(400, 90);
Feed(-40);
driveForward(127, -60, 160);
Feed(-127);
wait1Msec(1000);
Feed(0);
wait1Msec(5000000);
}
if(match==1 && skyrise==1 && blue==1)//---BLUE SKYRISE-----
{
SensorValue[latch] = 1;
wait1Msec(300);
armUp(160, 90);
autoFeed(300, -127);
autoFeed(1,0);
wait1Msec(500);
armDown(80, 90);
autoGrab(1);
armUp(185, 90);
liftMotors(-20);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(0, 90);
autoGrab(0); //drop one
armUp(180, 90);
liftMotors(-10);
DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(80, 90);
autoGrab(1);
armUp(220, 90);
DriveLoop(-1,-1, 925, 0.15, 0.00000001, 0.1, 120, 30);
armDown(180, 90);
autoGrab(0); //drop two
armUp(250, 90);
liftMotors(-15);
DriveLoop(1,1, 925, 0.15, 0.00000001, 0.1, 120, 30);
armDown(80, 90);
autoGrab(1);
armUp(340, 90);
DriveLoop(-1,-1, 920, 0.15, 0.00000001, 0.1, 120, 30);
armDown(320, 90);
autoGrab(0); //drop three
armUp(400, 90);
driveForward(-60, 120, 100);
Feed(-127);
driveForward(-60, 120, 170);
wait1Msec(5000);
Feed(0);
wait1Msec(5000000);
}
if(programmingSkills==1) //--------------------------------SKILLS-------------
{
SensorValue[latch] = 1;
armDown(0, 90);
wait1Msec(300);
armUp(130, 90);
autoFeed(300, -127);
autoFeed(1,0);
wait1Msec(500);
armDown(90, 90);
autoGrab(1);
armUp(300, 90);
liftMotors(-15);
DriveLoop(-1,-1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(0, 90);
autoGrab(0); //drop one
armUp(160, 90);
liftMotors(-10);
DriveLoop(1,1, 890, 0.15, 0.00000001, 0.1, 120, 30);
armDown(90, 90);
autoGrab(1);
armUp(200, 90);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(220, 90);
autoGrab(0); //drop two
armUp(290, 90);
liftMotors(-10);
DriveLoop(1,1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(90, 90);
autoGrab(1);
armUp(380, 90);
DriveLoop(-1,-1, 900, 0.15, 0.00000001, 0.1, 120, 30);
armDown(320, 90);
autoGrab(0); //drop three
armUp(370, 90);
liftMotors(-15);
DriveLoop(1,1, 905, 0.12, 0.00000001, 0.1, 80, 30);
armDown(90, 90);
autoGrab(1);
armUp(560, 90);
DriveLoop(-1,-1, 915, 0.12, 0.00000001, 0.1, 80, 30);
armDown(530, 90);
autoGrab(0); //drop four
armUp(610, 90);
liftMotors(-15);
DriveLoop(1,1, 915, 0.12, 0.00000001, 0.1, 80, 30);
armDown(90, 90);
autoGrab(1);
armUp(750, 90);
DriveLoop(-1,-1, 925, 0.12, 0.00000001, 0.1, 80, 30);
armDown(700, 90);
autoGrab(0); //drop five
armUp(750, 90);
liftMotors(-15);
DriveLoop(1,1, 950, 0.12, 0.00000001, 0.1, 80, 30);
armDown(90, 90);
autoGrab(1);
armUp(950, 90);
DriveLoop(-1,-1, 950, 0.11, 0.00000001, 0.1, 80, 30);
armDown(900, 90);
autoGrab(0); //drop six
armUp(950, 90);
liftMotors(-15);
DriveLoop(1,1, 950, 0.11, 0.00000001, 0.1, 80, 30);
armDown(90, 90);
autoGrab(1);
armUp(1220, 90);
DriveLoop(-1,-1, 900, 0.12, 0.00000001, 0.1, 80, 30);
armDown(1110, 90);
autoGrab(0); //drop seven
armUp(1220, 90);
/*liftMotors(100);
wait1Msec(500);
liftMotors(15);*/
driveForward(70, -70, 225);
driveForward(70, 70, 100);
autoFeed(750, -127);
SensorValue[conveyer] = 1;
driveForward(70, -70, 825);
armDown(20, 127);
liftMotors(-100);
wait1Msec(500);
liftMotors(-15);
driveForward(70, 70, 100);
autoFeed(1000, 127);
driveForward(70, -70, 225);
driveForward(70, 70, 100);
autoFeed(1000, 127);
driveForward(70, -70, 1000);
armUp(1220, 127);
driveForward(70, 70, 225);
autoFeed(750, -127);
wait1Msec(500);
driveForward(70, 70, 50);
autoFeed(750, -127);
driveForward(-70, -70, 200);
/*DriveLoop(1,-1, 250, 0.17, 0.00000001, 0.05, 120, 43);
DriveLoop(1,1, 140, 0.14, 0.00000001, 0.05, 80, 35);
autoFeed(1500, -127);
DriveLoop(-1,-1, 140, 0.14, 0.00000001, 0.05, 80, 35);
liftMotors(-50);
DriveLoop(-1,1, 350, 0.17, 0.00000001, 0.05, 120, 43);
liftMotors(-15);
DriveLoop(1,1, 825, 0.12, 0.00000001, 0.05, 80, 35);
DriveLoop(-1,1, 410, 0.12, 0.00000001, 0.05, 120, 43);
liftMotors(-127);
wait1Msec(1250);
liftMotors(-15);
DriveLoop(1,1, 175, 0.12, 0.00000001, 0.05, 80, 35);
autoFeed(750, 127);
wait1Msec(5000000);*/
}
if(match==1 && cube==1 && red==1) //-----------RED CUBES Match
{
DriveLoop(-1,1, 400, 0.12, 0.00000001, 0.05, 120, 50);
DriveLoop( 1,-1, 400, 0.12, 0.00000001, 0.05, 120, 50);
wait1Msec(50000000000);
/*SensorValue[latch] = 1;
driveStop(300);
armUp(1550, 90);
liftMotors(-10);
autoFeed(600, -127);
autoFeed(1,0);
wait1Msec(50);
armDown(1200, 120);
liftMotors(-127);
wait1Msec(250);
liftMotors(-15);
driveForward(100, 100, 425);
autoFeed(900, 127); //feed in red1
stopRobot(10);
liftMotors(60);
driveForward(100, 100, 350);
armUp(2300, 90);
driveForward(90, -60, 250);
autoFeed(800, -127); //feed out red1
autoFeed(1,0);
driveForward(-90, 60, 420);
armDown(1200, 110);
driveForward(100, 100, 100);
wait1Msec(50);
driveForward(70, -70, 125);
liftMotors(-127);
wait1Msec(450);
liftMotors(-15);
driveForward(100, 100, 650);
autoFeed(700, 127); //pick up blue
autoFeed(1, 0);
driveForward(-70, 70, 15);
autoFeed(375, 127); //move blue
autoFeed(1, 0);
driveForward(100, 100, 530);
autoFeed(600, 127); //pick up red2
driveForward(-70, 70, 10);
autoFeed(1, 0);
armUp(1800, 90);
driveForward(90, -60, 250);
wait1Msec(75);
autoFeed(550, -127); //drop red2
autoFeed(1,0);
driveBack(-90, -90, -100);
driveForward(-70, 70, 420);
armDown(1390, 90);
liftMotors(-10);
driveForward(90, 90, 400);*/
}
if(match==1 && cube==1 && blue==1) //-----------BLUE CUBES-------------
{
SensorValue[latch] = 1;
wait1Msec(300);
armUp(1550, 90);
liftMotors(-10);
autoFeed(600, -127);
autoFeed(1,0);
wait1Msec(50);
armDown(1425, 120);
liftMotors(-127);
wait1Msec(100);
liftMotors(-15);
SensorValue[conveyer] = 1;
driveForward(100, 100, 425);
autoFeed(925, 127); //feed in blue1
stopRobot(10);
liftMotors(40);
driveForward(100, 100, 415);
armUp(2300, 90);
driveForward(-90, 60, 265);
// driveForward(90, 90, 20);
autoFeed(800, -127); //feed out blue1
autoFeed(1,0);
// driveBack(-90, -90, -20);
driveForward(90, -60, 430);
liftMotors(-40);
driveForward(100, 100, 200);
driveForward(-70, 70, 90);
armDown(1425, 120);
liftMotors(-127);
wait1Msec(150);
liftMotors(-15);
driveForward(100, 100, 470);
autoFeed(850, 127); //pick up red
autoFeed(1, 0);
// driveForward(-70, 70, 15);
autoFeed(250, 127); //move red
autoFeed(1, 0);
driveForward(100, 100, 590);
autoFeed(650, 127); //pick up blue2
// driveForward(-70, 70, 10);
autoFeed(1, 0);
armUp(1800, 90);
driveForward(-90, 60, 290);
driveForward(90, 90, 40);
autoFeed(550, -127); //drop blue2
autoFeed(1,0);
driveBack(-90, -90, -100);
driveForward(-70, 70, 420);
armDown(1390, 90);
liftMotors(-10);
driveForward(90, 90, 400);
SensorValue[latch] = 1;
wait1Msec(300);
armUp(1550, 90);
liftMotors(-10);
autoFeed(600, -127);
autoFeed(1,0);
wait1Msec(50);
armDown(1200, 120);
liftMotors(-127);
wait1Msec(250);
liftMotors(-15);
driveForward(100, 100, 425);
autoFeed(900, 127); //feed in red1
stopRobot(10);
liftMotors(60);
driveForward(100, 100, 350);
armUp(2300, 90);
driveForward(-90, 60, 250);
autoFeed(800, -127); //feed out red1
autoFeed(1,0);
driveForward(90, -60, 420);
armDown(1450, 110);
driveForward(100, 100, 100);
wait1Msec(50);
driveForward(-70, 70, 85);
armDown(1200, 110);
liftMotors(-127);
wait1Msec(450);
liftMotors(-15);
driveForward(100, 100, 650);
autoFeed(700, 127); //pick up blue
autoFeed(1, 0);
// driveForward(-70, 70, 15);
autoFeed(375, 127); //move blue
autoFeed(1, 0);
driveForward(100, 100, 530);
autoFeed(600, 127); //pick up red2
// driveForward(-70, 70, 10);
autoFeed(1, 0);
armUp(1800, 90);
driveForward(-90, 60, 260);
wait1Msec(75);
autoFeed(550, -127); //drop red2
autoFeed(1,0);
driveBack(-90, -90, -100);
driveForward(-70, 70, 420);
armDown(1390, 90);
liftMotors(-10);
driveForward(90, 90, 400);
}
}
void classic15Red()
{
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int dead1 = 1000;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead (8);
//encoder values
int encoder1 = 1125; //drive to goal
int encoder2 = 325; //keep going towards goal
int encoder3 = 0; //drive slow, adjust to 90 degrees
int encoder4 = 275; //back up
int encoder5 = 800; //back up across the barrier again
int encoder6 = 90; // turn towards center large ball
int encoder7 = 220; // hit it off the barrier
int encoder8 = 350; // drive back
int encoder9 = 110; // turn towards other large ball
int encoder10 = 681; // hit it off the barrier
int encoder11 = 300; // drive back to square
int encoder12 = 150; // ass to wall
int encoder13 = 800;
int encoder14 = 400;
int encoder15 = 700;
int encoder16 = 400;
int encoder17 = 100;
int encoder18 = 400;
int encoder19 = 700;
int encoder20 = 400;
int encoder21 = 100;
int encoder22 = 400;
int encoder23 = 700;
// begin routine
intakeDead();
delay(1000);
stopIntake();
driveForward(encoder1); // drive to goal
armUp (goalHeight); //raise arm after cross barrier
driveForwardSlow(encoder2); //keep going towards goal
driveForwardSlowDead (dead1); //drive slow, adjust to 90 degrees
outtake (1500);
driveBack(encoder4); // back up
armDown(armMin); // lower arm
driveBack(encoder5); //back up across the barrier again
turnRight(encoder6); // turn towards center large ball
armUp(wallHeight); // arm up to wall height
driveForward(encoder7); // hit it off the barrier
driveBack(encoder8); // drive back
turnLeft(encoder9); // turn towards other large ball
driveForward(encoder10); // hit it off the barrier
driveBackSlowDead (dead2); // drive back to square
turnLeftArc (encoder12); // ass to wall
driveBackSlowDead (dead2) ; // wall align 90degrees
armDown (armMin); // arm down to floor
intakeDead (); // start intaking
driveForwardSlow (encoder13) ;// drive towards barf and intake whatever
turnRightArc (encoder14); // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder15); // drive to barrier
stopDrive(); // stop at barrier
outtake (1500); //drop the barf
driveBackSlowDead(dead2); //hump da bump
armDown (armMin); // arm down to floor
intakeDead (); //start intaking
turnLeftArc (encoder16); //face the barf
driveForwardSlow (encoder17) ;// drive towards barf and intake whatever
turnRightArc (encoder18) ; // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder19); // drive to barrier
stopDrive(); // stop at barrier
outtake (1500); //drop the barf
driveBackSlowDead(dead2); //drive to and align on bump
armDown (armMin); // arm down to floor
intakeDead (); //start intaking
turnLeftArc (encoder20); //face the barf
driveForwardSlow (encoder21) ;// drive towards barf and intake whatever
turnRightArc (encoder22) ; // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder23); // drive to barrier
outtake (1500); //drop the barf
//end of routine
stopAll () ;
delay(60000);///////////////////////////////////////////////////////////////////////////////////
}
