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 }
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 (); }
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 }
//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. }
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(); }
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); /////////////////////////////////////////////////////////////////////////////////// }
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; }
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 pickup(){ clawOpen(); armDown(); clawClose(); armUpHalfWay(); }
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 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; } }