From 83421db3aeb68a148467878d970466eb3b84e3b1 Mon Sep 17 00:00:00 2001 From: snydecor000 Date: Mon, 3 Feb 2025 19:27:10 -0500 Subject: [PATCH] Kettering Competition Changes: - Attempt to get path planner trajectory following working with 2025 libraries (Fail) - Add a 2 coral auto mode and paths - Modify PP Robot Config parameters - It seems like trajectories automatically reverse the states now (don't double reverse them) - Implement an L1 score coral task - Add an algae score height and button on the operator controller for it - Modify L3 and L4 height --- src/main/deploy/pathplanner/paths/Coral1.path | 54 ++++++++++++++ .../pathplanner/paths/Coral2Intake.path | 70 +++++++++++++++++++ .../deploy/pathplanner/paths/Coral2Score.path | 54 ++++++++++++++ src/main/deploy/pathplanner/paths/Long1.path | 2 +- .../deploy/pathplanner/paths/LongFake1.path | 2 +- .../pathplanner/paths/LongFake1Back.path | 2 +- .../deploy/pathplanner/paths/LongFake2.path | 2 +- .../pathplanner/paths/LongFake2Back.path | 2 +- .../deploy/pathplanner/paths/Note1Back.path | 2 +- src/main/deploy/pathplanner/paths/Note3.path | 2 +- .../deploy/pathplanner/paths/Note3Back.path | 2 +- src/main/deploy/pathplanner/settings.json | 32 +++++++++ src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/Robot.java | 5 +- .../java/frc/robot/autonomous/AutoRunner.java | 7 +- .../frc/robot/autonomous/modes/TwoCoral.java | 18 +++++ .../autonomous/tasks/DriveTrajectoryTask.java | 60 +++++++++------- .../autonomous/tasks/ScoreCoralTask.java | 30 ++++++++ .../controllers/OperatorController.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 8 +-- .../java/frc/robot/subsystems/Elevator.java | 9 ++- 21 files changed, 325 insertions(+), 46 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Coral1.path create mode 100644 src/main/deploy/pathplanner/paths/Coral2Intake.path create mode 100644 src/main/deploy/pathplanner/paths/Coral2Score.path create mode 100644 src/main/deploy/pathplanner/settings.json create mode 100644 src/main/java/frc/robot/autonomous/modes/TwoCoral.java create mode 100644 src/main/java/frc/robot/autonomous/tasks/ScoreCoralTask.java diff --git a/src/main/deploy/pathplanner/paths/Coral1.path b/src/main/deploy/pathplanner/paths/Coral1.path new file mode 100644 index 0000000..07c9475 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Coral1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.2, + "y": 4.020014204545454 + }, + "prevControl": null, + "nextControl": { + "x": 5.7900513452665265, + "y": 4.021582805878407 + }, + "isLocked": true, + "linkedName": null + }, + { + "anchor": { + "x": 5.7, + "y": 4.02039 + }, + "prevControl": { + "x": 6.2276773775961844, + "y": 4.02039 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "Coral1Score" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Coral2Intake.path b/src/main/deploy/pathplanner/paths/Coral2Intake.path new file mode 100644 index 0000000..9a88503 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Coral2Intake.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.7, + "y": 4.02039 + }, + "prevControl": null, + "nextControl": { + "x": 7.290098174329094, + "y": 4.02039 + }, + "isLocked": true, + "linkedName": "Coral1Score" + }, + { + "anchor": { + "x": 5.065568181818182, + "y": 1.7464914772727262 + }, + "prevControl": { + "x": 6.869912478592408, + "y": 1.5501638594890021 + }, + "nextControl": { + "x": 3.1410511363636364, + "y": 1.955894886363635 + }, + "isLocked": true, + "linkedName": null + }, + { + "anchor": { + "x": 1.4059943181818182, + "y": 0.6595880681818176 + }, + "prevControl": { + "x": 2.18156789016697, + "y": 1.7672219190942742 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "CoralIntake" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": true, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Coral2Score.path b/src/main/deploy/pathplanner/paths/Coral2Score.path new file mode 100644 index 0000000..6386a70 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Coral2Score.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4059943181818182, + "y": 0.6595880681818176 + }, + "prevControl": null, + "nextControl": { + "x": 1.9795707545328645, + "y": 1.4787401124708097 + }, + "isLocked": false, + "linkedName": "CoralIntake" + }, + { + "anchor": { + "x": 3.579801136363636, + "y": 2.77356534090909 + }, + "prevControl": { + "x": 3.1496913837504215, + "y": 2.0285933965521212 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Long1.path b/src/main/deploy/pathplanner/paths/Long1.path index ea4aa65..8a51413 100644 --- a/src/main/deploy/pathplanner/paths/Long1.path +++ b/src/main/deploy/pathplanner/paths/Long1.path @@ -109,7 +109,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Long", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/LongFake1.path b/src/main/deploy/pathplanner/paths/LongFake1.path index ff29643..da02099 100644 --- a/src/main/deploy/pathplanner/paths/LongFake1.path +++ b/src/main/deploy/pathplanner/paths/LongFake1.path @@ -61,7 +61,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "LongFake", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/LongFake1Back.path b/src/main/deploy/pathplanner/paths/LongFake1Back.path index 3a21f96..b740063 100644 --- a/src/main/deploy/pathplanner/paths/LongFake1Back.path +++ b/src/main/deploy/pathplanner/paths/LongFake1Back.path @@ -61,7 +61,7 @@ "rotation": 0.0 }, "reversed": true, - "folder": "LongFake", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/LongFake2.path b/src/main/deploy/pathplanner/paths/LongFake2.path index 630d3d3..edf6022 100644 --- a/src/main/deploy/pathplanner/paths/LongFake2.path +++ b/src/main/deploy/pathplanner/paths/LongFake2.path @@ -77,7 +77,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "LongFake", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/LongFake2Back.path b/src/main/deploy/pathplanner/paths/LongFake2Back.path index cffeff2..af9fb7b 100644 --- a/src/main/deploy/pathplanner/paths/LongFake2Back.path +++ b/src/main/deploy/pathplanner/paths/LongFake2Back.path @@ -77,7 +77,7 @@ "rotation": 0.0 }, "reversed": true, - "folder": "LongFake", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Note1Back.path b/src/main/deploy/pathplanner/paths/Note1Back.path index 6f9084b..e8f3a25 100644 --- a/src/main/deploy/pathplanner/paths/Note1Back.path +++ b/src/main/deploy/pathplanner/paths/Note1Back.path @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": true, - "folder": "PPTestMode", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Note3.path b/src/main/deploy/pathplanner/paths/Note3.path index f978f02..a685820 100644 --- a/src/main/deploy/pathplanner/paths/Note3.path +++ b/src/main/deploy/pathplanner/paths/Note3.path @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "PPTestMode", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Note3Back.path b/src/main/deploy/pathplanner/paths/Note3Back.path index 7b193ac..63d0f45 100644 --- a/src/main/deploy/pathplanner/paths/Note3Back.path +++ b/src/main/deploy/pathplanner/paths/Note3Back.path @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": true, - "folder": "PPTestMode", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..85c7bcd --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.8128, + "robotLength": 0.8382, + "holonomicMode": false, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 5.0, + "defaultMaxAccel": 2.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 55.0, + "robotMOI": 6.883, + "robotTrackwidth": 0.533, + "driveWheelRadius": 0.0762, + "driveGearing": 10.71, + "maxDriveSpeed": 5.45, + "driveMotorType": "NEO", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c940e95..6dc0f4a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,11 +27,11 @@ public static class Elevator { public static final double kStowHeight = 0.0; public static final double kL2Height = 9.0; - public static final double kL3Height = 25.14; - public static final double kL4Height = 52.0; + public static final double kL3Height = 26.14; + public static final double kL4Height = 54.5; public static final double kMaxHeight = 56.2; public static final double kGroundAlgaeHeight = 0.0; - public static final double kScoreAlgaeHeight = 0.0; + public static final double kScoreAlgaeHeight = 3.0; public static final double kLowAlgaeHeight = 24.8; public static final double kHighAlgaeHeight = 42.5; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fe4e8ab..f056e1e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -199,8 +199,9 @@ public void teleopPeriodic() { } else if (m_operatorController.getWantsStopAlgae()) { m_algae.stopAlgae(); m_algae.stow(); - } else if (m_operatorController.getWantsGroundAlgae()) { - m_algae.groundIntake(); + } else if (m_operatorController.getWantsScoreAlgae()) { + m_elevator.goToAlgaeScore(); + m_algae.grabAlgae(); } else if (m_operatorController.getWantsCoralIntake()) { m_coral.intake(); } diff --git a/src/main/java/frc/robot/autonomous/AutoRunner.java b/src/main/java/frc/robot/autonomous/AutoRunner.java index 3618ead..3499c35 100644 --- a/src/main/java/frc/robot/autonomous/AutoRunner.java +++ b/src/main/java/frc/robot/autonomous/AutoRunner.java @@ -8,6 +8,7 @@ import frc.robot.autonomous.modes.LongFakeAuto; import frc.robot.autonomous.modes.PPTestMode; import frc.robot.autonomous.modes.StressTest; +import frc.robot.autonomous.modes.TwoCoral; import frc.robot.autonomous.tasks.Task; public class AutoRunner { @@ -19,10 +20,11 @@ public class AutoRunner { public enum AutoMode { DO_NOTHING, DEFAULT, + TWO_CORAL, PP_TEST_MODE, STRESS_TEST, LONG_COOK, - LONG_FAKE_AUTO + LONG_FAKE_AUTO, } public static AutoRunner getInstance() { @@ -77,6 +79,9 @@ private void onAutoChange(String newAuto) { case LONG_FAKE_AUTO: m_autoMode = new LongFakeAuto(); break; + case TWO_CORAL: + m_autoMode = new TwoCoral(); + break; default: RobotTelemetry.print("Invalid auto mode selected. Defaulting to do nothing."); m_autoMode = new DoNothingMode(); diff --git a/src/main/java/frc/robot/autonomous/modes/TwoCoral.java b/src/main/java/frc/robot/autonomous/modes/TwoCoral.java new file mode 100644 index 0000000..5f900b5 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/modes/TwoCoral.java @@ -0,0 +1,18 @@ +package frc.robot.autonomous.modes; + +import frc.robot.autonomous.tasks.BrakeTask; +import frc.robot.autonomous.tasks.DriveTrajectoryTask; +import frc.robot.autonomous.tasks.ScoreCoralTask; +import frc.robot.autonomous.tasks.WaitTask; + +public class TwoCoral extends AutoModeBase { + public void queueTasks() { + queueTask(new DriveTrajectoryTask("Coral1")); + // queueTask(new DriveTrajectoryTask("Coral2Intake")); + // queueTask(new DriveTrajectoryTask("Coral2Score")); + queueTask(new WaitTask(1)); + queueTask(new ScoreCoralTask()); + + queueTask(new BrakeTask()); + } +} diff --git a/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java b/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java index 3262de8..935fc69 100644 --- a/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java +++ b/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java @@ -59,8 +59,12 @@ public DriveTrajectoryTask(String pathName) { m_isFinished = true; } - m_autoTrajectory = new PathPlannerTrajectory( - m_autoPath, + // m_autoTrajectory = new PathPlannerTrajectory( + // m_autoPath, + // new ChassisSpeeds(), + // m_drive.getPose().getRotation(), + // m_drive.getRobotConfig()); + m_autoTrajectory = m_autoPath.generateTrajectory( new ChassisSpeeds(), m_drive.getPose().getRotation(), m_drive.getRobotConfig()); @@ -88,41 +92,43 @@ public void start() { m_isFinished = false; Pose2d currentPose = m_drive.getPose(); + Logger.recordOutput("Auto/DriveTrajectory/ActualFirstPose", currentPose); + Logger.recordOutput("Auto/DriveTrajectory/ActualLastPose", m_autoTrajectory.getEndState().pose); ChassisSpeeds currentSpeeds = m_drive.getCurrentSpeeds(); m_driveController.reset(currentPose, currentSpeeds); - Rotation2d currentHeading = currentPose.getRotation(); - Rotation2d targetHeading; - if (m_autoPath.isReversed()) { - targetHeading = m_autoPath.getPoint(0).position.minus(m_autoPath.getPoint(1).position).getAngle(); - } else { - targetHeading = m_autoPath.getPoint(1).position.minus(m_autoPath.getPoint(0).position).getAngle(); - } - Rotation2d headingError = currentHeading.minus(targetHeading); + // Rotation2d currentHeading = currentPose.getRotation(); + // Rotation2d targetHeading; + // if (m_autoPath.isReversed()) { + // targetHeading = m_autoPath.getPoint(0).position.minus(m_autoPath.getPoint(1).position).getAngle(); + // } else { + // targetHeading = m_autoPath.getPoint(1).position.minus(m_autoPath.getPoint(0).position).getAngle(); + // } + // Rotation2d headingError = currentHeading.minus(targetHeading); - boolean onHeading = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond) < 0.25 - && Math.abs(headingError.getDegrees()) < 1; + // boolean onHeading = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond) < 0.25 + // && Math.abs(headingError.getDegrees()) < 1; - boolean onStartPose = currentPose.getTranslation().getDistance(m_autoPath.getPoint(0).position) < 0.25; + // boolean onStartPose = currentPose.getTranslation().getDistance(m_autoPath.getPoint(0).position) < 0.25; - boolean shouldReplan = !onStartPose || !onHeading; + // boolean shouldReplan = !onStartPose || !onHeading; - Logger.recordOutput("Auto/DriveTrajectory/shouldReplan", shouldReplan); + // Logger.recordOutput("Auto/DriveTrajectory/shouldReplan", shouldReplan); - if (shouldReplan) { - // TODO: maybe do this later? - // ... - // Or maybe not... - // replanPath(currentPose, currentSpeeds); - } + // if (shouldReplan) { + // // TODO: maybe do this later? + // // ... + // // Or maybe not... + // // replanPath(currentPose, currentSpeeds); + // } // DEBUG Trajectory ////////////////////////////////////////////// // Trajectory adjustedTrajectory = TrajectoryGenerator.generateTrajectory( // m_autoPath.getPathPoses(), // new TrajectoryConfig( - // m_autoPath.getGlobalConstraints().getMaxVelocityMps(), - // m_autoPath.getGlobalConstraints().getMaxAccelerationMpsSq())); + // m_autoPath.getGlobalConstraints().maxVelocityMPS(), + // m_autoPath.getGlobalConstraints().maxAccelerationMPSSq())); // Logger.recordOutput("Auto/DriveTrajectory/TargetTrajectory", // adjustedTrajectory); // if (shouldReplan) { @@ -133,6 +139,7 @@ public void start() { m_drive.clearTurnPIDAccumulation(); RobotTelemetry.print("Running path for " + DriverStation.getAlliance().toString()); + RobotTelemetry.print("Time Expected " + m_autoTrajectory.getTotalTimeSeconds() + " seconds"); m_runningTimer.reset(); m_runningTimer.start(); @@ -141,9 +148,9 @@ public void start() { @Override public void update() { PathPlannerTrajectoryState goal = m_autoTrajectory.sample(m_runningTimer.get()); - if (m_autoPath.isReversed()) { - goal = goal.reverse(); - } + // if (m_autoPath.isReversed()) { + // goal = goal.reverse(); + // } ChassisSpeeds chassisSpeeds = m_driveController.calculateRobotRelativeSpeeds(m_drive.getPose(), goal); m_drive.drive(chassisSpeeds); @@ -169,6 +176,7 @@ public void updateSim() { } public Pose2d getStartingPose() { + Logger.recordOutput("Auto/DriveTrajectory/FirstPose", m_autoPath.getStartingDifferentialPose()); return m_autoPath.getStartingDifferentialPose(); } diff --git a/src/main/java/frc/robot/autonomous/tasks/ScoreCoralTask.java b/src/main/java/frc/robot/autonomous/tasks/ScoreCoralTask.java new file mode 100644 index 0000000..7c22099 --- /dev/null +++ b/src/main/java/frc/robot/autonomous/tasks/ScoreCoralTask.java @@ -0,0 +1,30 @@ +package frc.robot.autonomous.tasks; + +import frc.robot.subsystems.Coral; + +public class ScoreCoralTask extends Task { + private Coral m_coral; + + public ScoreCoralTask() { + m_coral = Coral.getInstance(); + } + + @Override + public void start() { + m_coral.scoreL1(); + } + + @Override + public void update() { + } + + @Override + public boolean isFinished() { + return true; + } + + @Override + public void done() { + + } +} diff --git a/src/main/java/frc/robot/controls/controllers/OperatorController.java b/src/main/java/frc/robot/controls/controllers/OperatorController.java index d09bf09..7006a89 100644 --- a/src/main/java/frc/robot/controls/controllers/OperatorController.java +++ b/src/main/java/frc/robot/controls/controllers/OperatorController.java @@ -47,7 +47,7 @@ public boolean getWantsA2() { return this.getHatUp(); } - public boolean getWantsGroundAlgae() { + public boolean getWantsScoreAlgae() { return this.getHatLeft(); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 2446c02..36802d8 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -101,14 +101,14 @@ public class Drivetrain extends Subsystem { private final ModuleConfig moduleConfig = new ModuleConfig( Distance.ofBaseUnits(kWheelRadius, Meters), // wheel radius - LinearVelocity.ofBaseUnits(kMaxSpeed, MetersPerSecond), + LinearVelocity.ofBaseUnits(5, MetersPerSecond), 1.0, // coefficient of friction (1.0 is a placeholder value) - DCMotor.getCIM(2), - Current.ofBaseUnits(1, Amps), // Another placeholder + DCMotor.getNEO(2).withReduction(kGearRatio), + Current.ofBaseUnits(60, Amps), // Another placeholder 2); private final RobotConfig robotConfig = new RobotConfig( - Mass.ofBaseUnits(30, Kilogram), + Mass.ofBaseUnits(55, Kilogram), MomentOfInertia.ofBaseUnits(1, KilogramSquareMeters), moduleConfig, Distance.ofBaseUnits(mKinematics.trackWidthMeters, Meters)); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 8d3472e..11a5590 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -88,7 +88,8 @@ public enum ElevatorState { L3, L4, A1, - A2 + A2, + AS } private static class PeriodicIO { @@ -218,5 +219,11 @@ public void goToAlgaeHigh() { mPeriodicIO.state = ElevatorState.A2; } + public void goToAlgaeScore() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kScoreAlgaeHeight; + mPeriodicIO.state = ElevatorState.AS; + } + /*---------------------------------- Custom Private Functions ---------------------------------*/ }