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