void findLineLeft ()
{
int leftLine = analogRead(1);
int midLine = analogRead(2);
int rightLine = analogRead(3);
int black = 2000;
while ( midLine > black ) // black
{
printf("%d\r\n", midLine);
motorSet (motor1, 25); // drive slow until hit
motorSet (motor2, 25);
motorSet (motor9, 25);
motorSet (motor10, 25);
midLine = analogRead(2);
}
motorSet (motor1, -15); // no inertia
motorSet (motor2, -15);
motorSet (motor9, -15);
motorSet (motor10, -15);
delay(50);
stopDrive();
driveForwardSlow(60); // make sure it will be centered
stopDrive();
delay(160);
rightLine = analogRead(3);
midLine = analogRead(2);
leftLine = analogRead(1);
while (rightLine > black) // left is black
{
motorSet (motor1, 7); // turn left until mid line is set
motorSet (motor2, 7);
motorSet (motor9, 55);
motorSet (motor10, 55);
leftLine = analogRead(1);
midLine = analogRead(2);
rightLine = analogRead(3);
}
motorSet (motor1, -15); // no inertia
motorSet (motor2, -15);
motorSet (motor9, -15);
motorSet (motor10, -15);
delay(50);
stopDrive();
stopDrive();
delay(200);
}
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); ///////////////////////////////////////////////////////////////////////////////////
}
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);///////////////////////////////////////////////////////////////////////////////////
}
