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 homologation7(int color) {
unsigned int index = getMotionInstructionIndex();
switch (index) {
case 1:
takeBullion1(color);
break;
case 2:
cleanLintel1First( color);
break;
case 3:
cleanLintel1Second(color);
break;
case 4:
backToReadyForLintel1(color);
break;
case 5: // Open arm
armDown(color, ARM_RIGHT);
break;
case 6:
takeLintelLeft(color);
break;
case 7: // Rotation
left(color, 1700.0f);
break;
case 8: // Close ARM
armUp(color, ARM_RIGHT);
break;
case 9:
// go back home
spline(color, 0x0118, 0x016F, 0xFC7C, 0x33, 0x27, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
}
}
task main()
{
waitForStart();
moveForward(0.25, 80);
wait10Msec(50);
leftTwoWheelTurn(45, 50);
wait10Msec(56);
moveForward(41.5, 80);
wait10Msec(50);
rightTwoWheelTurn(45, 50);
wait10Msec(135);
//robot stopped in the third bucket from the right side of the pendulum
stopMotors();
wait10Msec(100);
/*
motor[tiltingMotor] = 75;
wait10Msec(98);
motor[tiltingMotor] = 25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
motor[conveyorMotor] = 100;
wait10Msec(200);
motor[conveyorMotor] = 0;
wait10Msec(50);
motor[tiltingMotor] = -35;
wait10Msec(105);
motor[tiltingMotor] = -25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);*/
armUp();
wait10Msec(200);
conveyorForward();
wait10Msec(200);
conveyorStop();
wait10Msec(100);
armDown();
wait10Msec(200);
leftTwoWheelTurn(42, 50);
wait10Msec(127);
moveForward(16, 80);
wait10Msec(50);
rightTwoWheelTurn(48, 50);
wait10Msec(53.5);
moveForward(25, 80);
wait10Msec(50);
rightTwoWheelTurn(48, 50);
wait10Msec(58);
moveForward(36, 80);
wait10Msec(50);
leftTwoWheelTurn(53, 50);
wait10Msec(115);
moveBackward(49, 80);
wait10Msec(50);
//robot parked in the middle of the ramp
}
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 Rotate::enter()
{
autoLog->log("Entering state 'Rotate' and rotating from " + std::to_string(getYaw()) + " to " + std::to_string(getYaw() + dest) + " degrees...");
startRot = getYaw();
armUp();
Wait(ARMUP_PULSETIME); //need to raise the arm a bit before rotating
armStop();
}
//Start of Auto
task main()
{
waitForStart(); //waits for Starts
initializeRobot();
//Servos
armUp();
forward(6950, 50);
stopMotors();
forward(650, 20);
// drives down the ramp
armUp();
frontServoDown();
wait1Msec(500);
turnLeft(4700, 20);
wait1Msec(500);
//turns to face the other tube
armUp();
backServoUp();
wait1Msec(500);
backwards(2375, 15); //backwards for half the distance as we want
stopMotors();
backServoDown();
armDown();
PlaySound(soundBeepBeep);
wait1Msec(500);
armUp();
//scores balls
frontServoDown();
backServoDown();
wait1Msec(500);
turnRight(300, 30);
//backs into the second tube and grabs it
wait1Msec(500);
PlaySound(soundBeepBeep);
forward(9000, 50);
turnRight(300, 30);
forward(450, 50);
turnLeft(500, 30);
wait1Msec(500);
stopMotors();
PlaySound(soundBeepBeep);
forward(5000, 7.5); //forward(5000, 20); //makes sure we dont fall short.
}
task main()
{
waitForStart();
stopMotors();
wait10Msec(500);
moveForward(2, 80);
wait10Msec(50);
rightTwoWheelTurn(10, 50);
wait10Msec(40);
//robot stops at first bucket from the right side of the pendulum
stopMotors();
wait10Msec(100);
/*
motor[tiltingMotor] = 75;
wait10Msec(105);
motor[tiltingMotor] = 25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
motor[conveyorMotor] = 100;
wait10Msec(200);
motor[conveyorMotor] = 0;
wait10Msec(50);
motor[tiltingMotor] = -45;
wait10Msec(110);
motor[tiltingMotor] = -25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);*/
armUp();
wait10Msec(200);
conveyorForward();
wait10Msec(200);
conveyorStop();
wait10Msec(100);
armDown();
wait10Msec(200);
moveForward(3, 80);
wait10Msec(50);
rightTwoWheelTurn(50, 50);
wait10Msec(83);
moveForward(25, 80);
wait10Msec(50);
leftTwoWheelTurn(48, 50);
wait10Msec(58);
moveForward(29, 80);
wait10Msec(50);
rightTwoWheelTurn(53, 50);
wait10Msec(150);
moveBackward(43, 80);
wait10Msec(60);
//robot parked in the middle of the ramp
}
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 ();
}
int main(){
armUp();
clawOpen();
camera_open(LOW_RES);
double start_wait=seconds();
while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds
int i=0;
while(i<10){//picks latest image from the buffer
camera_update();
i++;
}
if(get_object_count(chan)>0){
break;
}else{
stop(0.1);
}
}
while((seconds()-start_wait)<=60){
int area=get_object_area(chan, 0);
if(area>=600){
int i=0;
while(i<10){
camera_update();
i++;
}
int x=get_object_center(chan, 0).x;
if(x<65){
rightF(0.1, 100, 80);
}else{
leftF(0.1, 100, 80);
}
}else{
break;
}
}
armDown();
clawClose();
armUp();
}
task main()
{
waitForStart();
moveForward(2, 80);
wait10Msec(50);
leftTwoWheelTurn(45, 50);
wait10Msec(13);
//positioned in first bucket from the left side of the pendulum
stopMotors();
wait10Msec(50);
/*
motor[tiltingMotor] = 70;
wait10Msec(105);
motor[tiltingMotor] = 25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
motor[conveyorMotor] = 100;
wait10Msec(300);
motor[conveyorMotor] = 0;
wait10Msec(50);
motor[tiltingMotor] = -55;
wait10Msec(80);
motor[tiltingMotor] = -25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);*/
armUp();
wait10Msec(200);
conveyorForward();
wait10Msec(200);
conveyorStop();
wait10Msec(100);
armDown();
wait10Msec(200);
leftTwoWheelTurn(45, 50);
wait10Msec(60);
moveForward(54, 80);
wait10Msec(50);
leftTwoWheelTurn(90, 50);
wait10Msec(95);
moveBackward(46, 100);
wait10Msec(70);
//robot parked in the middle of the ramp
}
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 ();
}
int main()
{
//wait_for_light(1);//initialization
printf("version 1.9.5\n");
shut_down_in(115);
int servo_counter=0;
while(servo_counter<=3){
enable_servo(servo_counter);
servo_counter++;
}
// initialize routine
set_servo_position(0,2047);
armUp();
clawClose();
//get in front of transport
straight(1.8, 170);
rightC(0.28, 100);
leftC(0.28, -100);
straight(1.65, 170);
leftC(0.44, 100);
printf("completed first dead reckoning\n");
//until distance is 5 centimeter
/*
int claw_threshold = 13;
while(analog10(ETport)>claw_threshold){
back(0.1, -10);
msleep(1000);
printf("move once\n");
printf("%i\n", analog10(ETport));
}*/
back(0.25, -20);//pushes against transport
msleep(2000);
set_servo_position(0, 500);//puts claw on transport
msleep(1000);
printf("done with attaching to transport\n");
while(digital(8)==0)//goes forward with transport until it hits the wall
{
straight(0.1, 100);
msleep(10);
}
printf("at wall\n");
back(0.15, -50);//backs up from wall to deposit transport
set_servo_position(0, 2047);//raises claw
msleep(1500);
while(digital(8)==0){//goes forward until it hits wall for second time
straight(0.1, 100);
msleep(10);
}
printf("at wall 2\n");
back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall
leftC(0.6, 142);
int bump_counter=0;
while(digital(8)==0){//hits side wall
straight(0.1, 100);
msleep(10);
}
bump_counter++;//has one bump
int i=0;
while(i<10){//if hits side wall, pause
stop(0.2);
i++;
}
/*
==========================
==========End of routine 1======
KIPR at the wall
==========================
*/
// Pause here to wait for Create to drop POMS
msleep(1*1000);
//
int start=seconds();// start time
printf("will get out of corner\n");
back(0.5, -100);
while(seconds()-start<=3){//separates back from right angle
stop(0.1);
}
right_angle(left);
while(seconds()-start<=5){//separates right angle from straight
stop(0.1);
}
double start_drive=seconds();
straight(0.7, 100);//go to poms
stop(0.5);
camera_open(LOW_RES);
/*while(seconds()-start<=20){
int update_counter=0;
while(update_counter<10){
camera_update();
update_counter++;
}
if(get_object_count(chan)==0){
stop(0.1);
}
int x=get_object_center(chan, 0).x;
int area=get_object_area(chan, 0);
if(area<=400){
navigate(x);
}
if(area>=800){
//if objects are clumped are clumped, area will be automatically larger.
//this means that KIPR will start earlier than it should. We counter this here
//straight(0.18, 100);
straight(0.1, 100);
break;
}
}*/
stop(0.1);
printf("object seen\n");
double end_drive=seconds();//only for simulation. real end and start times would have to be taken from whole routine
//pick up POMS that are right there
pickup();
int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area
back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out
printf("at back wall\n");
straight(2.0, 100);//goes halway between border and back of launch area
right_angle(left);//faces transport
int starpof=seconds();//time after right angle
while(seconds()-starpof<=2){
motor(motorL, 0);
motor(motorR, 0);
}
while(digital(8)==0){
straight(0.1, 100);
}
straight(0.1, 100);
printf("touching transport\n");
motor(motorL, 0);
motor(motorR, 0);
dropoff();//drops poms in launch area
printf("poms in transport\n");
back(0.25, -100);
int starfop=seconds();
while(seconds()-starfop<=2){
motor(motorL, 0);
motor(motorR, 0);
}
int turns=1;
while(turns<=2){
right_angle(right);
stop(0.3);
turns++;
}
/*
======================
======Part 2 of routine 2====
======================
*/
while(1){
stop(0.5);//
while(digital(8)==0){
straight(0.1, 100);
}
back(0.3, -100);
right_angle(left);
pickup();
back((end_drive-start_drive)+1.5, -100);
straight(2.2, 100);
right_angle(left);
while(digital(8)==0){
straight(0.1, 100);
}
dropoff();
back(0.5, -100);
right_angle(left);
right_angle(left);
}
/*while(1){
start=0;
while(digital(8)==0){
straight(0.1, 80);
}
back(0.5, -100);
right_angle(right);
straight(0.5, 100);
right_angle(right);
right_angle(right);
while(seconds()-start<=20){
int update_counter=0;
while(update_counter<10){
camera_update();
update_counter++;
}
if(get_object_count(chan)==0){
stop(0.1);
}
int x=get_object_center(chan, 0).x;
int area=get_object_area(chan, 0);
if(area<=400){
navigate(x);
}
if(area>=800){
//if objects are clumped are clumped, area will be automatically larger.
//this means that KIPR will start earlier than it should. We counter this here
//straight(0.18, 100);
straight(0.1, 100);
break;
}
}
int servo_counter=1;
while(servo_counter<=3){
enable_servo(servo_counter);
printf("servo %d enabled\n", servo_counter);
servo_counter++;
}
pickup();
int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area
back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out
printf("at back wall\n");
straight(1.8, 100);//goes halway between border and back of launch area
right_angle(left);//faces transport
int starpof=seconds();//time after right angle
while(seconds()-starpof<=2){
motor(motorL, 0);
motor(motorR, 0);
}
while(digital(8)==0){
straight(0.1, 100);
}
straight(0.1, 100);
printf("touching transport\n");
motor(motorL, 0);
motor(motorR, 0);
dropoff();//drops poms in launch area
printf("poms in transport\n");
back(0.25, -100);
int starfop=seconds();
while(seconds()-starfop<=2){
motor(motorL, 0);
motor(motorR, 0);
}
int turns=1;
while(turns<=2){
right_angle(right);
turns++;
}
repeat_runs++;
}*/
while(1){
int servo_counter=0;//enables servos
while(servo_counter<=3){
enable_servo(servo_counter);
servo_counter++;
}
armUp();//raises arm at start
stop(0.5);
while(digital(TOUCH_SENSOR)==0){//go forward until left wall is contacted
straight(0.1, 100);
}
stop(0.1);
back(0.3, -100);//back up
stop(0.1);
right_angle2(left);//face poms
stop(0.1);
back(3+1.5, -100);//back up to rocket wall to straighten out
stop(0.1);
straight(3+1.5, 100);//go to poms
stop(0.1);
pickup();//pick poms up
stop(0.1);
back(3+1.5, -100);// back up until rocket wall
stop(0.1);
straight(2.2, 100);//go forward
stop(0.1);
right_angle2(left);//turn to face transport
stop(0.1);
while(digital(TOUCH_SENSOR)==0){//go forward until transport contacted
straight(0.1, 100);
}
stop(0.1);
dropoff();//drop poms off
stop(0.1);
back(0.5, -100);//back up from transpor
stop(0.1);
right_angle2(right);//orient for next iteration
stop(0.1);
right_angle2(right);
}
return 0;
}
int main()
{
printf("version 1.9.5\n");
shut_down_in(0.010*1000);
int start=seconds();//starting time for two minutes
enable_servo(0);
set_servo_position(0,2047);
straight(1.8, 170);//gets in front of transport
rightC(0.28, 100);
leftC(0.28, -100);
straight(1.65, 170);
leftC(0.44, 100);
printf("completed first dead reckoning\n");
set_analog_pullup(ETport, 0);
//until distance is 5 centimeter
/*
int claw_threshold = 13;
while(analog10(ETport)>claw_threshold){
back(0.1, -10);
msleep(1000);
printf("move once\n");
printf("%i\n", analog10(ETport));
}*/
back(0.25, -20);//pushes against transport
msleep(2000);
set_servo_position(0, 500);//puts claw on transport
msleep(1000);
printf("done with attaching to transport\n");
while(digital(8)==0)//goes forward with transport until it hits the wall
{
straight(0.1, 100);
msleep(10);
}
printf("at wall\n");
back(0.25, -50);//backs up from wall to deposit transport
set_servo_position(0, 2047);//raises claw
msleep(1500);
while(digital(8)==0){//goes forward until it hits wall for second time
straight(0.1, 100);
msleep(10);
}
printf("at wall 2\n");
back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall
leftC(0.6, 142);
int bump_counter=0;
while(digital(8)==0){//hits side wall
straight(0.1, 100);
msleep(10);
}
bump_counter++;//has one bump
int i=0;
while(i<10){//if hits side wall, pause
stop(0.2);
i++;
}
back(0.25, -100);
leftC(0.46, 100);
camera_open(LOW_RES);
armUp();
clawOpen();
double start_wait=seconds();
while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds
int i=0;
while(i<10){//picks latest image from the buffer
camera_update();
i++;
}
if(get_object_count(chan)>0){//if camera sees objects, skip the 25 seconds and go onto picking up stuff
break;
}else{//if camera doesn't see any objects, keep waiting until 25 seconds is up
stop(0.1);
}
}
int x=get_object_center(chan, 0).x;//declares global unchanging variable for the x location of the largest object
while((seconds()-start)<=120){//while there is still time left
int area=get_object_area(chan, 0);//creates local changing variable. this is th area of the largest object camera sees
if(area<=600){//if the object is small enough(far enough), navigate towards object. 600 is threshold
int i=0;
while(i<10){//buffer updating
camera_update();
i++;
}
navigate_poms(x);//similar to line followig function; gets to poms.
}
armDown();
clawClose();
armUp();
/*
leftF(0.5, 100, 80);//turn and go forward until transport is contacted
while(digital(8)==0){
straight(0.1, 100);*/
}
/*
back(1.5, 100);
leftC(0.44, 100);
while(digital(8)==0 && digital(9)==1){
straight(0.1, 100);
msleep(10);
if(digital(9)==0){
rightF(0.1, 100, 40);
left(0.1, 100);
msleep(10);
}
}
straight(2, 200);
rightC(0.44, 100);
straight(1, 80);
rightC(0.92, 100);
while(analog10(ETport<=600)){
msleep(10);
}
msleep(10000);
straight(1, 80);
leftC(0.44, 100);
straight(2, 200);
*/
return 0;
}
void initializeRobot()
{
armUp();
frontServoUp();
backServoUp();
}
task main()
{
waitForStart();
stopMotors();
wait10Msec(500);
moveForward(3, 80);
wait10Msec(50);
rightTwoWheelTurn(45, 50);
wait10Msec(58);
moveForward(44.5, 80);
wait10Msec(50);
leftTwoWheelTurn(45, 50);
wait10Msec(135);
//robot positioned at the third box from the left side of the pendulum
stopMotors();
wait10Msec(100);
/*
motor[tiltingMotor] = 75;
wait10Msec(105);
motor[tiltingMotor] = 25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
motor[conveyorMotor] = 100;
wait10Msec(350);
motor[conveyorMotor] = 0;
wait10Msec(50);
motor[tiltingMotor] = -35;
wait10Msec(105);
motor[tiltingMotor] = -25;
wait10Msec(10);
motor[tiltingMotor] = 0;
wait10Msec(10);*/
armUp();
wait10Msec(200);
conveyorForward();
wait10Msec(200);
conveyorStop();
wait10Msec(100);
armDown();
wait10Msec(200);
rightTwoWheelTurn(42, 50);
wait10Msec(143);
moveForward(13, 80);
wait10Msec(50);
leftTwoWheelTurn(48, 50);
wait10Msec(68);
moveForward(32, 80);
wait10Msec(50);
leftTwoWheelTurn(48, 50);
wait10Msec(50);
moveForward(30.5, 80);
wait10Msec(50);
rightTwoWheelTurn(53, 50);
wait10Msec(165);
moveBackward(44, 80);
wait10Msec(50);
//robot is parked in the ramp, in the middle
}
task main3(){
nMotorEncoder[armLeft2] = 0;
nMotorEncoder[armRight2] = 0;
//either for play
//flipout();
//or for testing
SensorValue[claw] = open;//close claw
wait1Msec(1000);//wait
intake();
wait1Msec(250);
armUp(250);//arm up just enough
wait1Msec(20);//wait
backRight(350*1.6);//first turn towards goal
right(175*1.6);//second turn towards goal
armDown(0);
wait1Msec(750);//wait
SensorValue[claw] = open;//open claw
wait1Msec(100);//wait
backLeft(175*1.6);//turn 1
wait1Msec(50);//wait
left(350*1.6);//turn 2
SensorValue[claw] = open;//close claw
wait1Msec(1000);//wait
intake();
wait1Msec(250);
armUp(250);//arm up just enough
wait1Msec(20);//wait
backRight(350*1.6);//first turn towards goal
right(200*1.6);//second turn towards goal
armDown(0);
wait1Msec(750);//wait
SensorValue[claw] = open;//open claw
wait1Msec(100);//wait
backLeft(160*1.6);//turn 1
wait1Msec(50);//wait
left(350*1.6);//turn 2
SensorValue[claw] = open;//close claw
wait1Msec(1000);//wait
intake();
wait1Msec(250);
armUp(250);//arm up just enough
wait1Msec(20);//wait
backRight(350*1.6);//first turn towards goal
right(200*1.6);//second turn towards goal
armDown(0);
/*wait1Msec(1000);//wait
intake();
wait1Msec(20);//wait .02 sec (future: is because this small thing stops bot from carrying its previous momentum forward)
armUp(200);//arm up just enough
wait1Msec(20);//wait
backRight(225);//first turn towards goal
right(235);//second turn towards goal
wait1Msec(500);//wait
armDown(0);
SensorValue[claw] = 0;//open claw
wait1Msec(500);//wait
armUp(150);
wait1Msec(20);//wait
backLeft(200);//turn 1
armDown(160);
wait1Msec(50);//wait
left(325);//turn 2
///REPEAT EVERYTHING
/*wait1Msec(750);//wait
SensorValue[claw] = 1;//close claw
wait1Msec(20);//wait .02 sec (future: because this small thing stops bot from carrying its previous momentum forward)
armUp(250);//arm up just enough
wait1Msec(20);//wait
//backRight(250);//first turn towards goal
//right(225);//second turn towards goal
backRight(275);//first turn towards goal
right(175);//second turn towards goal
wait1Msec(250);//wait
armDown(250);
SensorValue[claw] = 0;//open claw
wait1Msec(500);//wait
armUp(150);
wait1Msec(20);//wait
backLeft(225);
armDown(150);
wait1Msec(50);//wait
left(300);
///REPEAT EVERYTHING
wait1Msec(750);//wait*/
}
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);///////////////////////////////////////////////////////////////////////////////////
}
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 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 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 dropoff(){
clawOpen();
armUp();
clawClose();
}
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 homologation4(int color) {
unsigned int index = getMotionInstructionIndex();
switch (index) {
case 1:
takeBullion1(color);
break;
case 2:
// first bottle
bullion1ToBottle1(color);
break;
case 3: // Goto near 2 bottle
setSonarStatus(0); // TODO
bottle1ToFrontBottle2(color);
break;
case 4: // hit bottle 2
setSonarStatus(0);
frontBottle2ToBottle2(color);
break;
case 5: // near right Lintel 1
spline(color, X_LINTEL_RIGHT, 0x0580, ANGLE_NEG_90, 0x1E, 0x1E, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 6: // Open arm
armDown(color, ARM_RIGHT);
break;
case 7: // Go to Drop Zone 1
spline(color, 0x0402, 0x0115, ANGLE_NEG_90, 0x64, 0x32, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 8: // Close Arm
armUp(color, ARM_RIGHT);
break;
case 9: // Out from Drom Zone 1
// spline(color, 0x0315, 0x01A2, 0x04BA, 0xD9, 0xEC);
spline(color, 0x0480, 0x0230, ANGLE_NEG_110, 0xE0, 0xE0, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 10:
// 100° rotation
right(color, 1000.0f);
break;
case 11: // Clean the CD to be able to take Lintel
spline(color, X_LINTEL_LEFT - 50, 0x0440, ANGLE_90, 0x1A, 0x20, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 12: // Go back to open ARM
spline(color, X_LINTEL_LEFT, 0x02A0, ANGLE_90, 0xF0, 0xF0, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 13: // Open ARM
armDown(color, ARM_RIGHT);
break;
case 14: // take the left Lintel 1
spline(color, X_LINTEL_LEFT, 0x055C, ANGLE_90, 0x0D, 0x46, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 15: // Rotation
left(color, 1700.0f);
break;
case 16: // Close ARM
armUp(color, ARM_RIGHT);
break;
case 17:
// go back home
spline(color, 0x0118, 0x016F, 0xFC7C, 0x33, 0x27, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 18:
// go front home 1
spline(color, 0x0208, 0x02C2, 0xFC7C, 0xB6, 0x35, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 19: // Open ARM
armDown(color, ARM_RIGHT);
break;
case 20:
// take lintel left 2
spline(color, X_LINTEL_LEFT, 0x083C, ANGLE_90, 0x1E, 0x78, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 21: // Close ARM
armUp(color, ARM_RIGHT);
break;
case 22:
// take the 4 CD
spline(color, 0x06A8, 0x05D5, 0x0FC7C, 0x3E, 0x53, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
case 23:
// go back to drop Zone 2
spline(color, 0x038F, 0x00F0, 0x0FC7C, 0x3C, 0x1E, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
break;
}
}
