void autonFiveRight(){ //autonFiveLEft grabs the five stack, starts on left tile. SensorValue[rightIEM]=0; SensorValue[leftIEM]=0; strafeLeft(127, .3 * clickspermeters); drive( .1 * clickspermeters, FORWARD, 70); wrist(-127, 1100); //AArm(300 , LOWER, 60); wait1Msec(400); drive( .6 * clickspermeters, FORWARD, 80); wait1Msec(20); driveBackward(100, 100); StartTask(five); wrist(127, 600); wait1Msec(900); wrist(10, 20); aArm(1500, RAISE, 120); drive( .4 * clickspermeters, FORWARD, 70); wait1Msec(20); while(SensorValue[WristPot2] > 1900){ motor[leftWrist] = -127; motor[rightWrist] = -127; } motor[leftWrist] = 0; motor[rightWrist] = 0; driveBackward(100, 1000); wait1Msec(500); driveBackward(0, 1000); }
// TODO: use correct sensor port (change to irBack) task main() { calibrateGyro(); { // Drive to the IR basket, dump the block, drive back & square up against the wall int timeForward; ClearTimer(T1); driveBackward(DRIVE_SPEED); while (true) { if (time1[T1] > SEARCH_TIME) break; else if (SensorValue[irFront] == 2) break; } timeForward = time1[T1]; // Keep moving to adjust for the first 2 / last 2 baskets if (timeForward < MIDDLE_TIME) { // Back up a little driveStop(); wait1Msec(500); driveForward(FIRST_HALF_DELAY, DRIVE_SPEED); timeForward -= FIRST_HALF_DELAY; } else { // Back up a little driveStop(); wait1Msec(500); driveForward(SECOND_HALF_BACKUP_TIME, DRIVE_SPEED); timeForward -= SECOND_HALF_BACKUP_TIME; } driveStop(); flipper_flip(); driveForward(timeForward + 1500, DRIVE_SPEED); motor[rightDrive] = DRIVE_SPEED; wait1Msec(500); driveStop(); } { // Drive next to the ramp & turn on to it motor[leftDrive] = -DRIVE_SPEED; wait1Msec(1050); motor[rightDrive] = -DRIVE_SPEED; wait1Msec(1300); driveStop(); turnRightEuler(TURN_90_EULER, DRIVE_SPEED); PlaySound(soundBeepBeep); driveBackward(1500, DRIVE_SPEED); } }
// autonomous plays are named as follows auton[Score-Result][starting tile] void autonFiveLeft(){ //autonFiveLEft grabs the five stack, starts on left tile. SensorValue[rightIEM]=0; SensorValue[leftIEM]=0; strafeRight(127, .3 * clickspermeters); drive( .1 * clickspermeters, FORWARD, 70); wait1Msec(20); wrist(-127, 1100); //AArm(300 , LOWER, 60); wait1Msec(400); drive( .7 * clickspermeters, FORWARD, 80); StartTask(five); wrist(127, 600); wait1Msec(650); driveBackward(100, 150); wrist(10, 20); driveBackward(100, 300); //aArm(2100, RAISE, 120); while(SensorValue[ArmPot1] > 2100){ motor[leftTopArm] = 127; motor[leftBottomArm] = 127; motor[rightTopArm] = 127; motor[rightBottomArm] = 127; motor[rightWrist] = -27; motor[leftWrist] = -27; } motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; SensorValue[rightIEM]=0; SensorValue[leftIEM]=0; strafeLeft(100, clickspermeters); turnLeft(60, 600); drive( .37 * clickspermeters, FORWARD, 70); wait1Msec(20); while(SensorValue[WristPot1] > 200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } motor[leftWrist] = 0; motor[rightWrist] = 0; driveBackward(100, 300); wait1Msec(500); driveBackward(0, 20); }
void avoidObstacle() { driveBackward(); wait1Msec(1000); if (touchL != 0) { lastHitDir = -1; rightDeg(45); } else if (touchR != 0) { lastHitDir = 1; leftDeg(45); } driveForward(); wait1Msec(2000); }
task main() { servoPrep(); Joystick_WaitForStart(); disableDiagnosticsDisplay(); while(true) { encoderPrep(); startTrackers(); driveForward(310.0); turnRight(85.0); if(irDetected == true) { irPos2 = true; turnRight(40.0); irDetected = false; driveBackward(25.0); if(irDetected == true) { driveBackward(15.0); raiseLift(750.0); dropBallCenter(); wait1Msec(300); servo[centerServo] = endPosCenter - 30; lowerLift(1000.0); lowerLift(1000.0); lowerLift(1000.0); lowerLift(1000.0); lowerLift(1000.0); driveForward(105.0); turnRight(85.0); driveBackward(300.0); } else { irPos1 = true; driveBackward(110.0); turnRight(46.0); driveBackward(135.0); turnRight(3.0); raiseLift(750.0); dropBallCenter(); wait1Msec(300); servo[centerServo] = endPosCenter - 30; lowerLift(1000.0); lowerLift(1000.0); lowerLift(1000.0); lowerLift(1000.0); lowerLift(1000.0); driveForward(130.0); turnRight(82.0); driveBackward(300.0); } } else { driveForward(115.0); // ir loop if(irDetected == true) { irPos3 = true; driveBackward(15.0); raiseLift(750.0); dropBallCenter(); wait1Msec(300); servo[centerServo] = endPosCenter - 30; lowerLift(1000.0); lowerLift(1000.0); lowerLift(1000.0); lowerLift(1000.0); lowerLift(1000.0); driveForward(120.0); turnRight(85.0); driveBackward(300.0, 60); /*turnRight(43.0); driveBackward(250.0); motor[rightWheel] = -20; motor[leftWheel] = -20; wait1Msec(200); motor[rightWheel] = 0; motor[leftWheel] = 0; dropClamp(); driveForward(10.0); raiseLift(700.0); dropBallGoal(); resetDropGoal();*/ break; } else { driveBackward(220.0); turnRight(85.0); driveBackward(100.0); } } break; } }
int main(int argc, char** argv) { shutDownAfter(119); msleep(1000); //Wait for other robot to finish. Should be 30s (3000). Is only 1000 for testing raise_botguy_to(BOTGUY_CLAW_UP); enable_servos(); printf("camera open response: %i\n", camera_open()); printf("driving to pos.\n"); driveStraight(); msleep(3700); stop(); turnInPlaceCCW(); msleep(700); stop(); driveStraight(); msleep(1900); //was 1500 stop(); turnInPlaceCW(); msleep(580); stop(); msleep(1000); raise_botguy_to(BOTGUY_CLAW_DOWN); driveStraight(); msleep(1100); stop(); close_botguy_claw(); hold_botguy_claw_closed(); //preformApproachBotguy(false, 0); driveBackward(); msleep(700); raise_botguy_to(BOTGUY_CLAW_MID_UP); msleep(900); raise_botguy_to(BOTGUY_CLAW_FULL_UP); turnInPlaceCCW(); msleep(600); stop(); driveBackward(); msleep(1000); stop(); preformApproachCube(false, 1); stop(); return 0; turnInPlaceCW(); msleep(700); driveStraight(); msleep(3000); turnInPlaceCCW(); msleep(700); driveStraight(); msleep(1700); stop(); msleep(500); raise_botguy_to(BOTGUY_CLAW_MID_UP); return (EXIT_SUCCESS); }
void autonSkills(){ //autonFiveLEft grabs the five stack, starts on left tile. SensorValue[rightIEM]=0; SensorValue[leftIEM]=0; strafeRight(127, .3 * clickspermeters); drive( .1 * clickspermeters, FORWARD, 70); wait1Msec(20); wrist(-127, 1100); //AArm(300 , LOWER, 60); wait1Msec(400); drive( .7 * clickspermeters, FORWARD, 80); StartTask(five); wrist(127, 600); wait1Msec(600); driveBackward(100, 550); wait1Msec(20); while(SensorValue[ArmPot1] > 2100){ motor[leftTopArm] = 127; motor[leftBottomArm] = 127; motor[rightTopArm] = 127; motor[rightBottomArm] = 127; motor[rightWrist] = -27; motor[leftWrist] = -27; } driveBackward(0, 20); wait1Msec(50); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); while(SensorValue[ArmPot1] > 2100){ motor[leftTopArm] = 127; motor[leftBottomArm] = 127; motor[rightTopArm] = 127; motor[rightBottomArm] = 127; motor[rightWrist] = -27; motor[leftWrist] = -27; } driveBackward(0, 20); wait1Msec(50); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); SensorValue[rightIEM]=0; SensorValue[leftIEM]=0; strafeLeft(100, clickspermeters); wait1Msec(20); turnLeft(60, 600); wait1Msec(20); drive( .43 * clickspermeters, FORWARD, 70); wait1Msec(20); while(SensorValue[WristPot1] > 200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; driveBackward(100, 400); wait1Msec(500); driveBackward(0, 20); wait1Msec(20); while(SensorValue[WristPot1] < 2200){ motor[leftWrist] = 127; motor[rightWrist] = 127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; wait1Msec(20); while(SensorValue[ArmPot1] < 3900){ motor[leftTopArm] = -127; motor[leftBottomArm] = -127; motor[rightTopArm] = -127; motor[rightBottomArm] = -127; } wait1Msec(20); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); while(SensorValue[WristPot1] > 2200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; wait1Msec(20); turnLeft(60, 100); wait1Msec(20); drive( .37 * clickspermeters, FORWARD, 70); wait1Msec(20); while(SensorValue[WristPot1] <2800){ motor[leftWrist] = 127; motor[rightWrist] = 127; } wait1Msec(100); motor[leftWrist] = 0; motor[rightWrist] = 0; driveBackward(100, 650); wait1Msec(100); while(SensorValue[ArmPot1] > 2100){ motor[leftTopArm] = 127; motor[leftBottomArm] = 127; motor[rightTopArm] = 127; motor[rightBottomArm] = 127; motor[rightWrist] = -35; motor[leftWrist] = -35; } driveBackward(0, 20); wait1Msec(50); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; while(SensorValue[ArmPot1] > 2100){ motor[leftTopArm] = 127; motor[leftBottomArm] = 127; motor[rightTopArm] = 127; motor[rightBottomArm] = 127; motor[rightWrist] = -27; motor[leftWrist] = -27; } driveBackward(0, 20); wait1Msec(50); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); SensorValue[rightIEM]=0; SensorValue[leftIEM]=0; drive( .37 * clickspermeters, FORWARD, 70); wait1Msec(2000); while(SensorValue[WristPot1] > 200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; driveBackward(100, 850); wait1Msec(500); driveBackward(0, 20); wait1Msec(20); while(SensorValue[WristPot1] < 2200){ motor[leftWrist] = 127; motor[rightWrist] = 127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; wait1Msec(20); while(SensorValue[ArmPot1] < 3900){ motor[leftTopArm] = -127; motor[leftBottomArm] = -127; motor[rightTopArm] = -127; motor[rightBottomArm] = -127; } wait1Msec(20); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); while(SensorValue[WristPot1] > 2200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; wait1Msec(20); strafeRight(127, .9 * clickspermeters); turnRight(100, 200); while(SensorValue[ArmPot1] < 3900){ motor[leftTopArm] = -127; motor[leftBottomArm] = -127; motor[rightTopArm] = -127; motor[rightBottomArm] = -127; } wait1Msec(20); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); drive( .4 * clickspermeters, FORWARD, 70); wait1Msec(20); while(SensorValue[WristPot1] <2700){ motor[leftWrist] = 127; motor[rightWrist] = 127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; driveBackward(100, 650); wait1Msec(300); while(SensorValue[ArmPot1] > 2100){ motor[leftTopArm] = 127; motor[leftBottomArm] = 127; motor[rightTopArm] = 127; motor[rightBottomArm] = 127; motor[rightWrist] = -27; motor[leftWrist] = -27; } driveBackward(0, 20); wait1Msec(50); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; while(SensorValue[ArmPot1] > 2100){ motor[leftTopArm] = 127; motor[leftBottomArm] = 127; motor[rightTopArm] = 127; motor[rightBottomArm] = 127; motor[rightWrist] = -27; motor[leftWrist] = -27; } driveBackward(0, 20); wait1Msec(50); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); SensorValue[rightIEM]=0; SensorValue[leftIEM]=0; drive( .37 * clickspermeters, FORWARD, 70); wait1Msec(20); while(SensorValue[WristPot1] > 200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } motor[leftWrist] = 0; motor[rightWrist] = 0; wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; driveBackward(100, 650); wait1Msec(500); driveBackward(0, 20); wait1Msec(20); while(SensorValue[WristPot1] < 2200){ motor[leftWrist] = 127; motor[rightWrist] = 127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; wait1Msec(20); while(SensorValue[ArmPot1] < 3900){ motor[leftTopArm] = -127; motor[leftBottomArm] = -127; motor[rightTopArm] = -127; motor[rightBottomArm] = -127; } wait1Msec(20); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); while(SensorValue[WristPot1] > 2200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; wait1Msec(20); strafeLeft(127, .5 * clickspermeters); driveBackward(127 , 700); wait1Msec(5000); while(SensorValue[ArmPot1] < 3900){ motor[leftTopArm] = -127; motor[leftBottomArm] = -127; motor[rightTopArm] = -127; motor[rightBottomArm] = -127; } wait1Msec(20); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); while(SensorValue[WristPot1] > 2200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; wait1Msec(20); drive( .25 * clickspermeters, FORWARD, 70); while(SensorValue[ArmPot1] < 3900){ motor[leftTopArm] = -127; motor[leftBottomArm] = -127; motor[rightTopArm] = -127; motor[rightBottomArm] = -127; } wait1Msec(20); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); while(SensorValue[WristPot1] > 2200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } wait1Msec(20); motor[leftWrist] = 0; motor[rightWrist] = 0; wait1Msec(20); drive( .7 * clickspermeters, FORWARD, 70); wait1Msec(20) while(SensorValue[WristPot1] <2700){ motor[leftWrist] = 127; motor[rightWrist] = 127; } wait1Msec(100); motor[leftWrist] = 0; motor[rightWrist] = 0; driveBackward(100, 900); wait1Msec(10000); while(SensorValue[ArmPot1] > 2100){ motor[leftTopArm] = 127; motor[leftBottomArm] = 127; motor[rightTopArm] = 127; motor[rightBottomArm] = 127; motor[rightWrist] = -27; motor[leftWrist] = -27; } driveBackward(0, 20); wait1Msec(50); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; while(SensorValue[ArmPot1] > 2100){ motor[leftTopArm] = 127; motor[leftBottomArm] = 127; motor[rightTopArm] = 127; motor[rightBottomArm] = 127; motor[rightWrist] = -27; motor[leftWrist] = -27; } driveBackward(0, 20); wait1Msec(50); motor[leftTopArm] = 0; motor[leftBottomArm] = 0; motor[rightTopArm] = 0; motor[rightBottomArm] = 0; wait1Msec(20); strafeLeft(127, clickspermeters * .5); drive( 1.15 * clickspermeters, FORWARD, 70); while(SensorValue[WristPot1] > 200){ motor[leftWrist] = -127; motor[rightWrist] = -127; } motor[leftWrist] = 0; motor[rightWrist] = 0; }