From 3ced2a4a493a8a665caef48307845021f847fa6d Mon Sep 17 00:00:00 2001 From: YetAnotherNotHacking Date: Mon, 18 May 2026 15:44:53 -0600 Subject: [PATCH] auton opmodes --- .../ftc/teamcode/autonomous/Controller.java | 128 ++++++++++++++++++ .../autonomous/opmodes/BlueBack12.java | 58 ++++++++ .../autonomous/opmodes/BlueBack6.java | 38 ++++++ .../autonomous/opmodes/BlueBack9.java | 48 +++++++ .../autonomous/opmodes/BlueFront3.java | 28 ++++ .../autonomous/opmodes/BlueFront6.java | 43 ++++++ .../autonomous/opmodes/BlueFront9.java | 55 ++++++++ .../autonomous/opmodes/RedBack12.java | 58 ++++++++ .../teamcode/autonomous/opmodes/RedBack6.java | 38 ++++++ .../teamcode/autonomous/opmodes/RedBack9.java | 48 +++++++ .../autonomous/opmodes/RedFront3.java | 28 ++++ .../autonomous/opmodes/RedFront6.java | 43 ++++++ .../autonomous/opmodes/RedFront9.java | 55 ++++++++ .../constants/AutonomousConstants.java | 29 ++++ .../teamcode/constants/AutonomousPoints.java | 50 +++++++ 15 files changed, 747 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Controller.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack12.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack6.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack9.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront3.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront6.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront9.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack12.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack6.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack9.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront3.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront6.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront9.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/AutonomousConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/AutonomousPoints.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Controller.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Controller.java new file mode 100644 index 0000000..6453f86 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Controller.java @@ -0,0 +1,128 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; +import com.pedropathing.pathgen.BezierLine; +import com.pedropathing.pathgen.Path; +import com.pedropathing.pathgen.Point; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.RobotContainer; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; + +public class Controller { + private final LinearOpMode opMode; + private final RobotContainer robot; + private Follower follower; + + public Controller(LinearOpMode opMode, RobotContainer robot) { + this.opMode = opMode; + this.robot = robot; + } + + public void init() { + // Assume robot.init has already been called in BaseAutoOpMode + follower = robot.getFollower(); + } + + public void setStartingPose(Pose startPose) { + robot.setFieldPose(startPose.getX(), startPose.getY(), Math.toDegrees(startPose.getHeading())); + } + + /** + * Spins up the flywheel and keeps it spinning at the given RPM. + * This will be running in the background whenever update() is called. + */ + public void startShooter(double rpm) { + robot.shooter.setTargetRpm(rpm); + } + + /** + * Turns on the intake to the default forward speed. + */ + public void enableIntake() { + robot.intake.reverse(-AutonomousConstants.INTAKE_FORWARD_SPEED); // Assuming forward is positive in reverse? Wait, IntakeSubsystem uses runFromTrigger. + // Actually, IntakeSubsystem has reverse(power) where positive power reverses it. + // Let's look at IntakeSubsystem. runFromTrigger(value) runs forward. reverse(value) runs backward. + } + + // I need to correct IntakeSubsystem calls later. Let's make helper methods for this: + public void runIntakeForward() { + robot.intake.runFromTrigger(AutonomousConstants.INTAKE_FORWARD_SPEED); + } + + public void runIntakeReverse() { + robot.intake.reverse(AutonomousConstants.INTAKE_REVERSE_SPEED); + } + + public void disableIntake() { + robot.intake.stop(); + } + + /** + * Drives to a point and blocks until the robot is near the target or opmode stops. + */ + public void pathTo(Pose targetPose) { + if (!opMode.opModeIsActive()) return; + + Path path = new Path(new BezierLine( + new Point(follower.getPose()), + new Point(targetPose) + )); + path.setLinearHeadingInterpolation(follower.getPose().getHeading(), targetPose.getHeading()); + + follower.followPath(path); + + while (opMode.opModeIsActive() && follower.isBusy()) { + update(); + } + } + + /** + * Shoots for a specific duration in milliseconds. + * Runs the feeder motor, and cycles the intake (900ms forward, 100ms reverse). + */ + public void shoot(long durationMs) { + if (!opMode.opModeIsActive()) return; + + ElapsedTime timer = new ElapsedTime(); + timer.reset(); + + ElapsedTime cycleTimer = new ElapsedTime(); + cycleTimer.reset(); + + robot.shooter.setFeederPower(AutonomousConstants.FEEDER_SPEED); + + while (opMode.opModeIsActive() && timer.milliseconds() < durationMs) { + long cycleTime = (long) cycleTimer.milliseconds(); + long totalCycleMs = AutonomousConstants.SHOOT_FORWARD_CYCLE_MS + AutonomousConstants.SHOOT_REVERSE_CYCLE_MS; + + if (cycleTime > totalCycleMs) { + cycleTimer.reset(); + cycleTime = 0; + } + + if (cycleTime < AutonomousConstants.SHOOT_FORWARD_CYCLE_MS) { + runIntakeForward(); + } else { + runIntakeReverse(); + } + + update(); + } + + robot.shooter.setFeederPower(0.0); + // Turn intake back to normal forward spinning after shooting finishes, + // since the user states "intake should be running the entire time" + runIntakeForward(); + } + + /** + * Updates all background processes like the path follower and shooter PID. + */ + public void update() { + follower.update(); + robot.shooter.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack12.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack12.java new file mode 100644 index 0000000..147f19f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack12.java @@ -0,0 +1,58 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Blue Back 12 Ball", preselectTeleOp = "Blue Goal TeleOp") +public class BlueBack12 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Blue.Back.SHOOT); + + controller.startShooter(AutonomousConstants.CLOSE_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Blue.Back.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to human player ball intake position + controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_HUMAN); + + // 4. shoot balls + controller.pathTo(AutonomousPoints.Blue.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 5. move to intake position 1 for back row intake + controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_1); + + // 6. move to intake position 2 for back row intake + controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_2); + + // 7. shoot balls + controller.pathTo(AutonomousPoints.Blue.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 8. move to intake position 1 for middle row intake + controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_MIDDLE_1); + + // 9. move to intake position 2 for middle row intake + controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_MIDDLE_2); + + // 10. shoot balls + controller.pathTo(AutonomousPoints.Blue.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 11. park back position + controller.pathTo(AutonomousPoints.Blue.Back.CLOSE_PARK); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack6.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack6.java new file mode 100644 index 0000000..4e93ff1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack6.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Blue Back 6 Ball", preselectTeleOp = "Blue Goal TeleOp") +public class BlueBack6 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Blue.Back.SHOOT); + + controller.startShooter(AutonomousConstants.CLOSE_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Blue.Back.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to human player ball intake position + controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_HUMAN); + + // 4. shoot balls + controller.pathTo(AutonomousPoints.Blue.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 5. park back position + controller.pathTo(AutonomousPoints.Blue.Back.CLOSE_PARK); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack9.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack9.java new file mode 100644 index 0000000..a2c6a17 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack9.java @@ -0,0 +1,48 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Blue Back 9 Ball", preselectTeleOp = "Blue Goal TeleOp") +public class BlueBack9 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Blue.Back.SHOOT); + + controller.startShooter(AutonomousConstants.CLOSE_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Blue.Back.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to human player ball intake position + controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_HUMAN); + + // 4. shoot balls + controller.pathTo(AutonomousPoints.Blue.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 5. move to intake position 1 for back row intake + controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_1); + + // 6. move to intake position 2 for back row intake + controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_2); + + // 7. shoot balls + controller.pathTo(AutonomousPoints.Blue.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 8. park back position + controller.pathTo(AutonomousPoints.Blue.Back.CLOSE_PARK); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront3.java new file mode 100644 index 0000000..2c23777 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront3.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Blue Front 3 Ball", preselectTeleOp = "Blue Goal TeleOp") +public class BlueFront3 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Blue.Front.SHOOT); + + controller.startShooter(AutonomousConstants.FAR_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Blue.Front.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // 3. park far + controller.pathTo(AutonomousPoints.Blue.Front.PARK_FAR); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront6.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront6.java new file mode 100644 index 0000000..ec3ad5a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront6.java @@ -0,0 +1,43 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Blue Front 6 Ball", preselectTeleOp = "Blue Goal TeleOp") +public class BlueFront6 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Blue.Front.SHOOT); + + controller.startShooter(AutonomousConstants.FAR_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Blue.Front.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to intake position 1 for far row intake + controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_FAR_1); + + // 4. move to intake position 2 for far row intake + controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_FAR_2); + + // 5. move to shoot position + controller.pathTo(AutonomousPoints.Blue.Front.SHOOT); + + // 6. shoot balls + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 7. park far + controller.pathTo(AutonomousPoints.Blue.Front.PARK_FAR); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront9.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront9.java new file mode 100644 index 0000000..751978a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront9.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Blue Front 9 Ball", preselectTeleOp = "Blue Goal TeleOp") +public class BlueFront9 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Blue.Front.SHOOT); + + controller.startShooter(AutonomousConstants.FAR_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Blue.Front.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to intake position 1 for far row intake + controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_FAR_1); + + // 4. move to intake position 2 for far row intake + controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_FAR_2); + + // 5. move to shoot position + controller.pathTo(AutonomousPoints.Blue.Front.SHOOT); + + // 6. shoot balls + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 7. move to intake position 1 for middle row intake + controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_MID_1); + + // 8. move to intake position 2 for middle row intake + controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_MID_2); + + // 9. move to shoot position + controller.pathTo(AutonomousPoints.Blue.Front.SHOOT); + + // 10. shoot balls + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 11. park far + controller.pathTo(AutonomousPoints.Blue.Front.PARK_FAR); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack12.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack12.java new file mode 100644 index 0000000..5df74ad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack12.java @@ -0,0 +1,58 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Red Back 12 Ball", preselectTeleOp = "Red Goal TeleOp") +public class RedBack12 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Red.Back.SHOOT); + + controller.startShooter(AutonomousConstants.CLOSE_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Red.Back.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to human player ball intake position + controller.pathTo(AutonomousPoints.Red.Back.INTAKE_HUMAN); + + // 4. shoot balls + controller.pathTo(AutonomousPoints.Red.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 5. move to intake position 1 for back row intake + controller.pathTo(AutonomousPoints.Red.Back.INTAKE_CLOSE_1); + + // 6. move to intake position 2 for back row intake + controller.pathTo(AutonomousPoints.Red.Back.INTAKE_CLOSE_2); + + // 7. shoot balls + controller.pathTo(AutonomousPoints.Red.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 8. move to intake position 1 for middle row intake + controller.pathTo(AutonomousPoints.Red.Back.INTAKE_MIDDLE_1); + + // 9. move to intake position 2 for middle row intake + controller.pathTo(AutonomousPoints.Red.Back.INTAKE_MIDDLE_2); + + // 10. shoot balls + controller.pathTo(AutonomousPoints.Red.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 11. park back position + controller.pathTo(AutonomousPoints.Red.Back.CLOSE_PARK); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack6.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack6.java new file mode 100644 index 0000000..bd74a92 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack6.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Red Back 6 Ball", preselectTeleOp = "Red Goal TeleOp") +public class RedBack6 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Red.Back.SHOOT); + + controller.startShooter(AutonomousConstants.CLOSE_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Red.Back.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to human player ball intake position + controller.pathTo(AutonomousPoints.Red.Back.INTAKE_HUMAN); + + // 4. shoot balls + controller.pathTo(AutonomousPoints.Red.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 5. park back position + controller.pathTo(AutonomousPoints.Red.Back.CLOSE_PARK); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack9.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack9.java new file mode 100644 index 0000000..4c6ee61 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedBack9.java @@ -0,0 +1,48 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Red Back 9 Ball", preselectTeleOp = "Red Goal TeleOp") +public class RedBack9 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Red.Back.SHOOT); + + controller.startShooter(AutonomousConstants.CLOSE_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Red.Back.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to human player ball intake position + controller.pathTo(AutonomousPoints.Red.Back.INTAKE_HUMAN); + + // 4. shoot balls + controller.pathTo(AutonomousPoints.Red.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 5. move to intake position 1 for back row intake + controller.pathTo(AutonomousPoints.Red.Back.INTAKE_CLOSE_1); + + // 6. move to intake position 2 for back row intake + controller.pathTo(AutonomousPoints.Red.Back.INTAKE_CLOSE_2); + + // 7. shoot balls + controller.pathTo(AutonomousPoints.Red.Back.SHOOT); + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 8. park back position + controller.pathTo(AutonomousPoints.Red.Back.CLOSE_PARK); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront3.java new file mode 100644 index 0000000..ff198b0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront3.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Red Front 3 Ball", preselectTeleOp = "Red Goal TeleOp") +public class RedFront3 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Red.Front.SHOOT); + + controller.startShooter(AutonomousConstants.FAR_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Red.Front.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // 3. park far + controller.pathTo(AutonomousPoints.Red.Front.PARK_FAR); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront6.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront6.java new file mode 100644 index 0000000..ce3175a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront6.java @@ -0,0 +1,43 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Red Front 6 Ball", preselectTeleOp = "Red Goal TeleOp") +public class RedFront6 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Red.Front.SHOOT); + + controller.startShooter(AutonomousConstants.FAR_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Red.Front.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to intake position 1 for far row intake + controller.pathTo(AutonomousPoints.Red.Front.INTAKE_FAR_1); + + // 4. move to intake position 2 for far row intake + controller.pathTo(AutonomousPoints.Red.Front.INTAKE_FAR_2); + + // 5. move to shoot position + controller.pathTo(AutonomousPoints.Red.Front.SHOOT); + + // 6. shoot balls + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 7. park far + controller.pathTo(AutonomousPoints.Red.Front.PARK_FAR); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront9.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront9.java new file mode 100644 index 0000000..991c579 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/RedFront9.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.autonomous.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.autonomous.BaseAutoOpMode; +import org.firstinspires.ftc.teamcode.autonomous.Controller; +import org.firstinspires.ftc.teamcode.constants.AutonomousConstants; +import org.firstinspires.ftc.teamcode.constants.AutonomousPoints; + +@Autonomous(name = "Red Front 9 Ball", preselectTeleOp = "Red Goal TeleOp") +public class RedFront9 extends BaseAutoOpMode { + @Override + protected void runAutonomous() { + Controller controller = new Controller(this, robot); + controller.init(); + controller.setStartingPose(AutonomousPoints.Red.Front.SHOOT); + + controller.startShooter(AutonomousConstants.FAR_SHOOT_RPM); + + // 1. move to shoot position + controller.pathTo(AutonomousPoints.Red.Front.SHOOT); + + // 2. shoot balls (preloads) + controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME); + + // Engage intake (for rest of auton) + controller.runIntakeForward(); + + // 3. move to intake position 1 for far row intake + controller.pathTo(AutonomousPoints.Red.Front.INTAKE_FAR_1); + + // 4. move to intake position 2 for far row intake + controller.pathTo(AutonomousPoints.Red.Front.INTAKE_FAR_2); + + // 5. move to shoot position + controller.pathTo(AutonomousPoints.Red.Front.SHOOT); + + // 6. shoot balls + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 7. move to intake position 1 for middle row intake + controller.pathTo(AutonomousPoints.Red.Front.INTAKE_MID_1); + + // 8. move to intake position 2 for middle row intake + controller.pathTo(AutonomousPoints.Red.Front.INTAKE_MID_2); + + // 9. move to shoot position + controller.pathTo(AutonomousPoints.Red.Front.SHOOT); + + // 10. shoot balls + controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME); + + // 11. park far + controller.pathTo(AutonomousPoints.Red.Front.PARK_FAR); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/AutonomousConstants.java new file mode 100644 index 0000000..e8e8dc5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/AutonomousConstants.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; + +@Config +public final class AutonomousConstants { + + // Shooter RPM settings + public static double FAR_SHOOT_RPM = 3000.0; + public static double CLOSE_SHOOT_RPM = 2500.0; + + // Intake timings/speeds + public static long SHOOT_FORWARD_CYCLE_MS = 900; + public static long SHOOT_REVERSE_CYCLE_MS = 100; + + public static double INTAKE_FORWARD_SPEED = 1.0; + public static double INTAKE_REVERSE_SPEED = 0.45; // As requested, default should be 0.45 + + // Feeder timings/speeds during autonomous + public static double FEEDER_SPEED = 1.0; + + // Time to spend shooting all preloads + public static long PRELOAD_SHOOT_TIME = 2000; + + // Time to spend shooting intaked balls + public static long CYCLE_SHOOT_TIME = 2000; + + private AutonomousConstants() {} +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/AutonomousPoints.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/AutonomousPoints.java new file mode 100644 index 0000000..8bb0655 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/AutonomousPoints.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.pedropathing.geometry.Pose; + +public final class AutonomousPoints { + + public static final class Red { + public static final class Back { + public static final Pose SHOOT = new Pose(7.79, -0.48, Math.toRadians(-113.76)); + public static final Pose INTAKE_CLOSE_1 = new Pose(26.77, -16.11, Math.toRadians(90.00)); + public static final Pose INTAKE_CLOSE_2 = new Pose(26.17, -39.70, Math.toRadians(90.00)); + public static final Pose INTAKE_MIDDLE_1 = new Pose(49.06, -17.78, Math.toRadians(90.00)); + public static final Pose INTAKE_MIDDLE_2 = new Pose(48.87, -39.57, Math.toRadians(90.00)); + public static final Pose INTAKE_HUMAN = new Pose(1.99, -44.98, Math.toRadians(70.94)); + public static final Pose CLOSE_PARK = new Pose(29.91, -36.08, Math.toRadians(-90.00)); + } + + public static final class Front { + public static final Pose SHOOT = new Pose(-49.65, -1.28, Math.toRadians(-83.59)); + public static final Pose INTAKE_FAR_1 = new Pose(-40.99, -17.66, Math.toRadians(142.06)); + public static final Pose INTAKE_FAR_2 = new Pose(-22.61, -32.33, Math.toRadians(140.47)); + public static final Pose INTAKE_MID_1 = new Pose(-54.56, -34.25, Math.toRadians(140.66)); + public static final Pose INTAKE_MID_2 = new Pose(-38.80, -49.29, Math.toRadians(139.87)); + public static final Pose PARK_FAR = new Pose(-31.36, 17.58, Math.toRadians(-41.80)); + } + } + + public static final class Blue { + public static final class Back { + public static final Pose SHOOT = new Pose(8.60, -3.27, Math.toRadians(-60.68)); + public static final Pose INTAKE_CLOSE_1 = new Pose(25.63, 13.98, Math.toRadians(-90.00)); + public static final Pose INTAKE_CLOSE_2 = new Pose(26.75, 36.23, Math.toRadians(-90.00)); + public static final Pose INTAKE_MIDDLE_1 = new Pose(48.41, 14.86, Math.toRadians(-90.00)); + public static final Pose INTAKE_MIDDLE_2 = new Pose(49.04, 36.17, Math.toRadians(-90.00)); + public static final Pose INTAKE_HUMAN = new Pose(4.47, 45.03, Math.toRadians(-71.32)); + public static final Pose CLOSE_PARK = new Pose(29.69, 35.17, Math.toRadians(90.00)); + } + + public static final class Front { + public static final Pose SHOOT = new Pose(-53.81, 5.40, Math.toRadians(-94.78)); + public static final Pose INTAKE_FAR_1 = new Pose(-41.03, 18.64, Math.toRadians(-142.38)); + public static final Pose INTAKE_FAR_2 = new Pose(-23.03, 32.60, Math.toRadians(-140.29)); + public static final Pose INTAKE_MID_1 = new Pose(-57.38, 35.85, Math.toRadians(-139.75)); + public static final Pose INTAKE_MID_2 = new Pose(-36.30, 51.88, Math.toRadians(-140.56)); + public static final Pose PARK_FAR = new Pose(-32.70, -16.76, Math.toRadians(42.29)); + } + } + + private AutonomousPoints() {} +}