diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 9f6f396..7e13cf4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -1,67 +1,7 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveH; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveX; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveY; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveH; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveX; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveY; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; @@ -849,6 +789,7 @@ public class Auto_LT_Close extends LinearOpMode { if (gamepad2.squareWasPressed()){ robot.limelight.start(); robot.limelight.pipelineSwitch(1); + gamepad2.rumble(500); } if (redAlliance) { @@ -1052,8 +993,8 @@ public class Auto_LT_Close extends LinearOpMode { shootAllVelocity, shootAllHood, intake1Time, - x2b, - y2b, + 0.501, + 0.501, pickup1XTolerance, pickup1YTolerance ), @@ -1113,8 +1054,8 @@ public class Auto_LT_Close extends LinearOpMode { pickup2.build(), manageShooterAuto( intake2Time, - x2b, - y2b, + 0.501, + 0.501, pickup1XTolerance, pickup1YTolerance ), @@ -1171,8 +1112,8 @@ public class Auto_LT_Close extends LinearOpMode { pickup3.build(), manageShooterAuto( intake3Time, - x2b, - y2b, + 0.501, + 0.501, pickup1XTolerance, pickup1YTolerance ), diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 8f1c859..1b34049 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -1,13 +1,7 @@ package org.firstinspires.ftc.teamcode.autonomous; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveH; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveX; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveY; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveH; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveX; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveY; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleEnd; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; @@ -31,6 +25,7 @@ import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.ParallelAction; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -81,6 +76,12 @@ public class Auto_LT_Far extends LinearOpMode { public static double firstShootTime = 0.3; public static double flywheel0Time = 3.5; public static double shoot0Time = 2; + boolean gatePickup = false; + boolean stack3 = true; + double xStackPickupA, yStackPickupA, hStackPickupA; + double xStackPickupB, yStackPickupB, hStackPickupB; + public static int pickupStackSpeed = 15; + int prevMotif = 0; public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { return new Action() { @@ -667,6 +668,9 @@ public class Auto_LT_Far extends LinearOpMode { // initialize path variables here TrajectoryActionBuilder leave3Ball = null; + TrajectoryActionBuilder leaveFromShoot = null; + TrajectoryActionBuilder pickup3 = null; + TrajectoryActionBuilder shoot3 = null; @Override public void runOpMode() throws InterruptedException { @@ -694,23 +698,34 @@ public class Auto_LT_Far extends LinearOpMode { turret.setTurret(turrDefault); - drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); + drive = new MecanumDrive(hardwareMap, autoStart); servos.setSpinPos(autoSpinStartPos); servos.setTransferPos(transferServo_out); - robot.light.setPosition(1); - while (opModeInInit()) { // Recalibration in initialization drive.updatePoseEstimate(); - if (gamepad2.square) { - teleEnd = drive.localizer.getPose(); // use this position as starting position + if (gamepad2.triangle) { + autoStart = drive.localizer.getPose(); // use this position as starting position gamepad2.rumble(1000); } + if (gamepad2.squareWasPressed()){ + robot.limelight.start(); + robot.limelight.pipelineSwitch(1); + gamepad2.rumble(500); + } + + if (gamepad2.leftBumperWasPressed()){ + gatePickup = !gatePickup; + } + if (gamepad2.rightBumperWasPressed()){ + stack3 = !stack3; + } + turret.setTurret(turretShootPos); robot.hood.setPosition(shoot0Hood); @@ -734,6 +749,18 @@ public class Auto_LT_Far extends LinearOpMode { yLeave = rLeaveY; hLeave = rLeaveH; + xShoot = rShootX; + yShoot = rShootY; + hShoot = rShootH; + + xStackPickupA = rStackPickupAX; + yStackPickupA = rStackPickupAY; + hStackPickupA = rStackPickupAH; + + xStackPickupB = rStackPickupBX; + yStackPickupB = rStackPickupBY; + hStackPickupB = rStackPickupBH; + turretShootPos = turrDefault + redTurretShootPos; } else { robot.light.setPosition(0.6); @@ -742,15 +769,40 @@ public class Auto_LT_Far extends LinearOpMode { yLeave = bLeaveY; hLeave = bLeaveH; + xShoot = bShootX; + yShoot = bShootY; + hShoot = bShootH; + + xStackPickupA = bStackPickupAX; + yStackPickupA = bStackPickupAY; + hStackPickupA = bStackPickupAH; + + xStackPickupB = bStackPickupBX; + yStackPickupB = bStackPickupBY; + hStackPickupB = bStackPickupBH; + turretShootPos = turrDefault + blueTurretShootPos; } - leave3Ball = drive.actionBuilder(teleEnd) + leave3Ball = drive.actionBuilder(autoStart) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + leaveFromShoot = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + + pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA)) + .strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB), + new TranslationalVelConstraint(pickupStackSpeed)); + + shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); - TELE.addData("Start Position", teleEnd); + TELE.addData("Gate Cycle?", gatePickup); + TELE.addData("Pickup Stack?", stack3); + TELE.addData("Start Position", autoStart); TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift TELE.update(); } @@ -762,11 +814,21 @@ public class Auto_LT_Far extends LinearOpMode { // Currently only shoots; keep this start and modify times and then add extra paths if (opModeIsActive()) { + double stamp = getRuntime(); + robot.transfer.setPower(1); startAuto(); - leave3Ball(); + if (stack3){ + cycleStackFar(); + } + + if (gatePickup || stack3){ + leave(); + } else { + leave3Ball(); + } // Actual way to end autonomous in to find final position while (opModeIsActive()) { @@ -810,4 +872,67 @@ public class Auto_LT_Far extends LinearOpMode { assert leave3Ball != null; Actions.runBlocking(leave3Ball.build()); } + + void leave(){ + assert leaveFromShoot != null; + Actions.runBlocking(leaveFromShoot.build()); + } + + void cycleStackFar(){ + Actions.runBlocking( + new ParallelAction( + pickup3.build(), + manageShooterAuto( + intake3Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + intake(intake3Time), + detectObelisk( + intake3Time, + 0.501, + 0.501, + 0.501, + 0.501, + obeliskTurrPos3 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot3Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot3.build(), + prepareShootAll(colorSenseTime, shoot3Time, motif) + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + finalShootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) + ) + + ); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java index b0fd2fa..b4ca67a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.autonomous; +package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java index bd45591..c32dc38 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.autonomous; +package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 0488a76..c091ccd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -1,10 +1,23 @@ package org.firstinspires.ftc.teamcode.constants; import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; @Config public class Back_Poses { - public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50; + public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1; public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50; + public static double rShootX = 95, rShootY = 85, rShootH = 90; + public static double bShootX = 95, bShootY = -85, bShootH = -90; + + public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140; + public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; + + public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1; + public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1; + + public static Pose2d autoStart = new Pose2d(0, 0, 0); // TODO: find this position + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index bb7d29b..63f7f6e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -43,5 +43,4 @@ public class Front_Poses { public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; public static Pose2d teleStart = new Pose2d(0, 0, 0); - public static Pose2d teleEnd = new Pose2d(0, 0, 0); }