From 2fcc757bbb7348ce1b4965b0fee113868819fcbd Mon Sep 17 00:00:00 2001 From: Robert Teal Date: Thu, 21 Dec 2023 13:05:44 -0600 Subject: [PATCH] created a new abstract auto and example red backstage auto that are simplified versions of the current autos utilizing only state machines instead of the step class and builder functions --- .../ftc/teamcode/hardware/Robot.java | 1 - .../teamcode/opmode/autonomous/AutoBase.java | 61 +++++++++ .../opmode/autonomous/RedBackStageAuto.java | 127 ++++++++++++++++++ 3 files changed, 188 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 6f0c7ab..fec2c7b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -26,7 +26,6 @@ public class Robot { public int lastMacro = 0; private boolean camEnabled = true; - public static double slideWait = 1.5; public static double scoreWait = 0.65; public static double armWait = 2.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java new file mode 100644 index 0000000..fbcb826 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java @@ -0,0 +1,61 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; + +public abstract class AutoBase extends LinearOpMode { + public Robot robot; + + protected int macroState; + protected double macroTime; + protected int teamPropLocation = 2; // 1 is left, 2 is middle, 3 is right, perhaps these could be enums instead + + // abstract methods for each auto to implement + public abstract void createTrajectories(); + public abstract void followTrajectories(); + + @Override + public void runOpMode() { + // create telemetry object for both driver hub and dashboard + // check SampleMecanumDrive to make sure it doesn't override the dashboard telemetry + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + // initialize robot + robot = new Robot(hardwareMap); + + // create trajectories + createTrajectories(); + + // wait for start + while (!isStarted() || !isStopRequested()) { + if (robot.camera.getFrameCount() < 1) { + telemetry.addLine("Initializing..."); + } else { + telemetry.addLine("Initialized"); + teamPropLocation = robot.camera.getMarkerId(); // or whatever method you end up using + telemetry.addData("Team Prop Location", teamPropLocation); + } + telemetry.update(); + } + + // run auto + while (macroState != -1 && !isStopRequested()) { + // state machine to follow trajectories + followTrajectories(); + + // update robot + robot.update(getRuntime()); + + // update telemetry + telemetry.addLine(robot.getTelemetry()); + telemetry.update(); + } + + // stop camera + robot.camera.stopTargetingCamera(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java new file mode 100644 index 0000000..072df29 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java @@ -0,0 +1,127 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.hardware.Slides; + +@Autonomous(name = "Red Backstage Auto", group = "Competition", preselectTeleOp = "Main TeleOp") +public class RedBackStageAuto extends AutoBase { + public Trajectory scorePurple1; + public Trajectory scorePurple2; + public Trajectory scorePurple3; + + public Trajectory scoreYellow1; + public Trajectory scoreYellow2; + public Trajectory scoreYellow3; + + public Trajectory parkRobot1; + public Trajectory parkRobot2; + public Trajectory parkRobot3; + + @Override + public void createTrajectories() { + // set start position + Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90)); + robot.drive.setPoseEstimate(start); + + // create pose2d variables + // you might not need 3 instances of the deposit position, for example, however based on localization accuracy + // you might need them for each one to be slightly different + Pose2d drop1 = new Pose2d(12, -40.5, Math.toRadians(90)); + Pose2d drop2 = new Pose2d(12, -40.5, Math.toRadians(90)); + Pose2d drop3 = new Pose2d(12, -40.5, Math.toRadians(90)); + + Pose2d depositPreload1 = new Pose2d(46, -36, Math.toRadians(180)); + Pose2d depositPreload2 = new Pose2d(46, -36, Math.toRadians(180)); + Pose2d depositPreload3 = new Pose2d(46, -36, Math.toRadians(180)); + + Pose2d park1 = new Pose2d(48, -12, Math.toRadians(180)); + Pose2d park2 = new Pose2d(48, -12, Math.toRadians(180)); + Pose2d park3 = new Pose2d(48, -12, Math.toRadians(180)); + + // create trajectories + scorePurple1 = robot.drive.trajectoryBuilder(start) + .lineToLinearHeading(drop1) + .build(); + scorePurple2 = robot.drive.trajectoryBuilder(start) + .lineToLinearHeading(drop2) + .build(); + scorePurple3 = robot.drive.trajectoryBuilder(start) + .lineToLinearHeading(drop3) + .build(); + + scoreYellow1 = robot.drive.trajectoryBuilder(scorePurple1.end()) + .lineToLinearHeading(depositPreload1) + .build(); + scoreYellow2 = robot.drive.trajectoryBuilder(scorePurple2.end()) + .lineToLinearHeading(depositPreload2) + .build(); + scoreYellow3 = robot.drive.trajectoryBuilder(scorePurple3.end()) + .lineToLinearHeading(depositPreload3) + .build(); + + parkRobot1 = robot.drive.trajectoryBuilder(scoreYellow1.end()) + .lineToLinearHeading(park1) + .build(); + parkRobot2 = robot.drive.trajectoryBuilder(scoreYellow2.end()) + .lineToLinearHeading(park2) + .build(); + parkRobot3 = robot.drive.trajectoryBuilder(scoreYellow3.end()) + .lineToLinearHeading(park3) + .build(); + } + + @Override + public void followTrajectories() { + switch (macroState) { + case 0: + // start drive to tape + robot.drive.followTrajectoryAsync(teamPropLocation==1?scorePurple1:(teamPropLocation==2?scorePurple2:scorePurple3)); + macroState++; + break; + case 1: + // wait until robot is at tape + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + macroState++; + } + case 2: + // run intake + if (getRuntime() < macroTime + 0.5) { + robot.intake.setDcMotor(-0.3); + } + // start drive to backdrop + else { + robot.extendMacro(Slides.Position.TIER1, getRuntime()); + robot.drive.followTrajectoryAsync(teamPropLocation==1?scoreYellow1:(teamPropLocation==2?scoreYellow2:scoreYellow3)); + macroState++; + } + break; + case 3: + // extend macro + if (robot.macroState != 0) { + robot.extendMacro(Slides.Position.TIER1, getRuntime()); + } + // if macro and drive are done, start park + else if (!robot.drive.isBusy()) { + robot.resetMacro(Slides.Position.DOWN, getRuntime()); + robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3)); + macroState++; + } + break; + // park robot + case 4: + // reset macro + if (robot.macroState != 0) { + robot.resetMacro(Slides.Position.DOWN, getRuntime()); + } + // if macro and drive are done, end auto + if (!robot.drive.isBusy()) { + macroState = -1; + } + break; + } + } +} \ No newline at end of file