From aadc2cfda76eb695c5cd19e9849cba06d42c83b8 Mon Sep 17 00:00:00 2001 From: "Liam P." Date: Wed, 5 Feb 2025 20:45:36 -0500 Subject: [PATCH] 146 bind auto triggers (#151) * Added triggers/commands Added triggers for target arm positioning and aligning, started commands to enact them. * Finished Auto Triggers Also contains test path (Test-Auto Auto) * Misc. Changes, Constants for Offset * Shaved excess imports * SpotlessApply * Modified to add scoring * weird changes Co-authored-by: Liam P. * Corrected/simplified triggers * refactored triggers to be named commands and corrected deadline logic * Updated with correct logic * formatted * final teeny eenie little bittle changes * yay finalized stuff --------- Co-authored-by: mmilunicmobile Co-authored-by: Liam P. Co-authored-by: Matthew Milunic <62996888+mmilunicmobile@users.noreply.github.com> --- .../pathplanner/autos/Test Half Field.auto | 6 + .../pathplanner/autos/Test-Auto Auto.auto | 55 +++++ .../pathplanner/autos/test matthew yay.auto | 61 ++++++ .../paths/Red C_L4-SourceRight.path | 73 +++++++ .../paths/Red SourceRight-C_L4.path | 73 +++++++ .../paths/Red StartRight-SourceRight .path | 73 +++++++ .../deploy/pathplanner/paths/ab to bc.path | 54 +++++ .../pathplanner/paths/source to ab.path | 54 +++++ src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/Auto.java | 198 +++++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 62 ++++-- .../robot/commands/AlignAndDriveToReef.java | 8 +- .../robot/constants/AligningConstants.java | 4 + .../SuperstructureStateManager.java | 11 +- .../swerve/CommandSwerveDrivetrain.java | 2 - 15 files changed, 714 insertions(+), 24 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Test-Auto Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/test matthew yay.auto create mode 100644 src/main/deploy/pathplanner/paths/Red C_L4-SourceRight.path create mode 100644 src/main/deploy/pathplanner/paths/Red SourceRight-C_L4.path create mode 100644 src/main/deploy/pathplanner/paths/Red StartRight-SourceRight .path create mode 100644 src/main/deploy/pathplanner/paths/ab to bc.path create mode 100644 src/main/deploy/pathplanner/paths/source to ab.path diff --git a/src/main/deploy/pathplanner/autos/Test Half Field.auto b/src/main/deploy/pathplanner/autos/Test Half Field.auto index d21439b7..5581ca3c 100644 --- a/src/main/deploy/pathplanner/autos/Test Half Field.auto +++ b/src/main/deploy/pathplanner/autos/Test Half Field.auto @@ -9,6 +9,12 @@ "data": { "pathName": "Half-Field Sprint" } + }, + { + "type": "named", + "data": { + "name": "height L4" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Test-Auto Auto.auto b/src/main/deploy/pathplanner/autos/Test-Auto Auto.auto new file mode 100644 index 00000000..70025bc0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test-Auto Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red StartRight-SourceRight " + } + }, + { + "type": "path", + "data": { + "pathName": "Red SourceRight-C_L4" + } + }, + { + "type": "named", + "data": { + "name": "wait" + } + }, + { + "type": "path", + "data": { + "pathName": "Red C_L4-SourceRight" + } + }, + { + "type": "named", + "data": { + "name": "wait" + } + }, + { + "type": "path", + "data": { + "pathName": "Red SourceRight-C_L4" + } + }, + { + "type": "named", + "data": { + "name": "wait" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/test matthew yay.auto b/src/main/deploy/pathplanner/autos/test matthew yay.auto new file mode 100644 index 00000000..69b78923 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/test matthew yay.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "source to ab" + } + }, + { + "type": "named", + "data": { + "name": "location B" + } + }, + { + "type": "named", + "data": { + "name": "height L4" + } + }, + { + "type": "named", + "data": { + "name": "place" + } + }, + { + "type": "path", + "data": { + "pathName": "ab to bc" + } + }, + { + "type": "named", + "data": { + "name": "location C" + } + }, + { + "type": "named", + "data": { + "name": "height L2" + } + }, + { + "type": "named", + "data": { + "name": "place" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red C_L4-SourceRight.path b/src/main/deploy/pathplanner/paths/Red C_L4-SourceRight.path new file mode 100644 index 00000000..50254b82 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red C_L4-SourceRight.path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 13.978, + "y": 5.422496749024708 + }, + "prevControl": null, + "nextControl": { + "x": 15.821846999144302, + "y": 5.225778445369686 + }, + "isLocked": false, + "linkedName": "Red Reef CD" + }, + { + "anchor": { + "x": 16.216089743589745, + "y": 6.966346153846153 + }, + "prevControl": { + "x": 15.522569813923187, + "y": 6.416928142732595 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Source Right" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "location C", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "height L4", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "place", + "waypointRelativePos": 1.0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 54.381510160748206 + }, + "reversed": false, + "folder": "AutoTest", + "idealStartingState": { + "velocity": 0, + "rotation": -122.10625595511773 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red SourceRight-C_L4.path b/src/main/deploy/pathplanner/paths/Red SourceRight-C_L4.path new file mode 100644 index 00000000..928b695d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red SourceRight-C_L4.path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 16.216089743589745, + "y": 6.966346153846153 + }, + "prevControl": null, + "nextControl": { + "x": 15.471575600017074, + "y": 5.982294727572125 + }, + "isLocked": false, + "linkedName": "Red Source Right" + }, + { + "anchor": { + "x": 13.978, + "y": 5.422496749024708 + }, + "prevControl": { + "x": 15.773854545219386, + "y": 5.5667511706492 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Reef CD" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "location C", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "height L4", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "place", + "waypointRelativePos": 1.0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -122.10625595511773 + }, + "reversed": false, + "folder": "AutoTest", + "idealStartingState": { + "velocity": 0, + "rotation": 54.381510160748206 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red StartRight-SourceRight .path b/src/main/deploy/pathplanner/paths/Red StartRight-SourceRight .path new file mode 100644 index 00000000..6ed916fe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red StartRight-SourceRight .path @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 9.972179487179487, + "y": 6.243910256410256 + }, + "prevControl": null, + "nextControl": { + "x": 10.777520702180267, + "y": 6.29031630098106 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 16.216089743589745, + "y": 6.966346153846153 + }, + "prevControl": { + "x": 13.887348178338486, + "y": 6.651331408074669 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Source Right" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "location Source Right", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "height Source", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "align", + "waypointRelativePos": 1.0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 54.381510160748206 + }, + "reversed": false, + "folder": "AutoTest", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ab to bc.path b/src/main/deploy/pathplanner/paths/ab to bc.path new file mode 100644 index 00000000..5394e284 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ab to bc.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.2307954545454542, + "y": 4.000071022727272 + }, + "prevControl": null, + "nextControl": { + "x": 2.3532954545454547, + "y": 3.3419460227272717 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6795170454545456, + "y": 2.663877840909091 + }, + "prevControl": { + "x": 2.6795170454545456, + "y": 2.663877840909091 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 60.06848815949221 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -0.9710219310791925 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/source to ab.path b/src/main/deploy/pathplanner/paths/source to ab.path new file mode 100644 index 00000000..bcc9fa63 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/source to ab.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0270738636363637, + "y": 1.0484801136363637 + }, + "prevControl": null, + "nextControl": { + "x": 1.3760795454545454, + "y": 2.3447869318181827 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8319318181818183, + "y": 3.940241477272727 + }, + "prevControl": { + "x": 1.8319318181818183, + "y": 3.940241477272727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.4320961841646833 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 55.09241514460653 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 35a007c5..c52b9e29 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,7 +2,9 @@ "robotWidth": 0.914, "robotLength": 0.914, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "AutoTest" + ], "autoFolders": [], "defaultMaxVel": 4.5, "defaultMaxAccel": 3.0, diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java index ba3ca965..317e932c 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/robot/Auto.java @@ -1,6 +1,7 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; @@ -8,6 +9,9 @@ import com.pathplanner.lib.trajectory.PathPlannerTrajectory; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.util.Units; @@ -17,11 +21,18 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.constants.AligningConstants; import frc.robot.constants.GlobalConstants; +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.ModeManager.SuperstructureStateManager.SuperstructureState; +import frc.robot.subsystems.ModeManager.SuperstructureStateManager.SuperstructureState.Position; import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; import java.util.ArrayList; import java.util.List; import java.util.Optional; +import java.util.Set; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class Auto { @@ -30,11 +41,18 @@ public class Auto { private Alliance previousAlliance = Alliance.Blue; private RobotConfig config; // PathPlanner robot configuration + // #146 + private RobotContainer robotContainer; + private DriveLocation targetLocation = DriveLocation.GH; + private ArmHeight targetHeight = ArmHeight.Home; + // *NEW private final Field2d m_trajectoryField = new Field2d(); - public Auto(CommandSwerveDrivetrain drivetrain) { + public Auto(CommandSwerveDrivetrain drivetrain, RobotContainer robotContainer) { + this.robotContainer = robotContainer; setUpPathPlanner(drivetrain); + configureBindings(); autoChooser = new LoggedDashboardChooser<>("Auto Routine", AutoBuilder.buildAutoChooser()); SmartDashboard.putData("Auto Path", m_trajectoryField); } @@ -149,4 +167,182 @@ public void setUpPathPlanner(CommandSwerveDrivetrain drivetrain) { m_trajectoryField.getObject("Robot").setPose(pose); }); } + + // #146 Constants + public static double leftOffset = AligningConstants.leftOffset; + public static double rightOffset = AligningConstants.rightOffset; + public static double centerOffset = AligningConstants.centerOffset; + + public enum DriveLocation { + SourceLeft(1, 13, 0), + SourceRight(2, 12, 0), + A(7, 18, leftOffset), + AB(7, 18, centerOffset), + B(7, 18, rightOffset), + C(8, 17, leftOffset), + CD(8, 17, centerOffset), + D(8, 17, rightOffset), + E(9, 22, leftOffset), + EF(9, 22, centerOffset), + F(9, 22, rightOffset), + G(10, 21, leftOffset), + GH(10, 21, centerOffset), + H(10, 21, rightOffset), + I(11, 20, leftOffset), + JH(11, 20, centerOffset), + J(11, 20, rightOffset), + K(6, 19, leftOffset), + KL(6, 19, centerOffset), + L(6, 19, rightOffset); + + public int tagRed; + public int tagBlue; + public double offset; + + private DriveLocation(int tagRed, int tagBlue, double offset) { + this.tagRed = tagRed; + this.tagBlue = tagBlue; + this.offset = offset; + } + + public int getTagByTeam() { + Optional ally = DriverStation.getAlliance(); + if (ally.isPresent()) { + if (ally.get() == Alliance.Red) return tagRed; + if (ally.get() == Alliance.Blue) return tagBlue; + } + + return tagRed; + } + } + + public enum ArmHeight { + Home(Position.Home, Position.Home), + L1(Position.L1, Position.L1Prep), + L2(Position.L2, Position.L2Prep), + L3(Position.L3, Position.L3Prep), + L4(Position.L4, Position.L4Prep), + Source(Position.Source, Position.SourcePrep); + + public Position position; + public double armMotorSpeed; + public Position prep; + + private ArmHeight(Position position, Position prep) { + this.position = position; + this.prep = prep; + } + } + + // #146: Add a function that will register all triggers + private double placeTimeout = 0.5; + + public void configureBindings() { + NamedCommands.registerCommand( + "wait", Commands.waitUntil(() -> armInPlace() && robotInPlace())); + + Command alignCommand = + Commands.defer( + () -> + robotContainer.alignAndDriveToReef( + targetLocation.getTagByTeam(), targetLocation.offset), + Set.of(robotContainer.drivetrain)); + NamedCommands.registerCommand("align", alignCommand); + + Command armCommand = + Commands.defer( + () -> robotContainer.stateManager.moveToPosition(targetHeight.position), + Set.of( + robotContainer.armSubsystem, + robotContainer.elevatorSubsystem, + robotContainer.stateManager)); + NamedCommands.registerCommand("arm", armCommand.asProxy()); + + Command prepArmCommand = + Commands.defer( + () -> robotContainer.stateManager.moveToPosition(targetHeight.prep), + Set.of( + robotContainer.armSubsystem, + robotContainer.elevatorSubsystem, + robotContainer.stateManager)); + NamedCommands.registerCommand("preparm", prepArmCommand.asProxy()); + + Command scoreCommand = + robotContainer.gripperSubsystem.ejectSpinCoral().withTimeout(placeTimeout); + NamedCommands.registerCommand("score", scoreCommand.asProxy()); + + Command intakeCommand = robotContainer.intakeSubsystem.intake(); + NamedCommands.registerCommand("intake", intakeCommand.asProxy()); + + // spotless:off + Command placeCommand = + prepArmCommand.asProxy() + .until(() -> robotInPlace()) // Wait until arm and align are in position + .andThen( + ( + Commands.waitUntil(() -> armInPlace()) + .andThen(scoreCommand.asProxy()) + ) + .deadlineFor(armCommand.asProxy()) + ) + .deadlineFor(alignCommand); + NamedCommands.registerCommand("place", placeCommand); + // spotless:on + + // #region Auto create locations and heights + for (DriveLocation location : DriveLocation.values()) { + NamedCommands.registerCommand( + "location ".concat(location.name()), + Commands.runOnce( + () -> { + targetLocation = location; + Logger.recordOutput("Auto/Chosen Location", location); + })); + } + + for (ArmHeight height : ArmHeight.values()) { + NamedCommands.registerCommand( + "height ".concat(height.name()), + Commands.runOnce( + () -> { + targetHeight = height; + Logger.recordOutput("Auto/Chosen Height", height); + })); + } + // #endregion + + } + + @AutoLogOutput(key = "Auto/Arm In Place") + private boolean armInPlace() { + return SuperstructureState.AUTO.isAtTarget( + targetHeight.position, robotContainer.stateManager); + } + + @AutoLogOutput(key = "Auto/Arm In Prep") + private boolean armInPrep() { + return SuperstructureState.AUTO.isAtTarget(targetHeight.prep, robotContainer.stateManager); + } + + @AutoLogOutput(key = "Auto/Robot In Place") + private boolean robotInPlace() { + Pose2d alignmentPose = + VisionConstants.aprilTagLayout + .getTagPose(targetLocation.getTagByTeam()) + .get() + .toPose2d() + .plus( + new Transform2d( + new Translation2d( + Units.feetToMeters(3) / 2, targetLocation.offset), + Rotation2d.k180deg)); + Logger.recordOutput("Auto/Physical Target Pose", alignmentPose); + Pose2d currentPose = robotContainer.drivetrain.getRobotPose(); + Pose2d relativePos = alignmentPose.relativeTo(currentPose); + Logger.recordOutput("Auto/Physical Relative Pose", relativePos); + return (Math.abs(relativePos.getX()) < Units.inchesToMeters(0.7)) + && (Math.abs(relativePos.getY()) < Units.inchesToMeters(0.7)) + && ((Math.abs(relativePos.getRotation().getRadians()) % Math.PI) + < Units.degreesToRadians(2)); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cdcba59e..0a86b32d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -20,6 +23,7 @@ import frc.robot.commands.AlignToPiece; import frc.robot.commands.AlignToReef; import frc.robot.commands.WheelRadiusCharacterization; +import frc.robot.constants.AligningConstants; import frc.robot.constants.GlobalConstants; import frc.robot.constants.GlobalConstants.ControllerConstants; import frc.robot.constants.TunerConstants; @@ -64,7 +68,7 @@ public class RobotContainer { public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - public Auto auto = new Auto(drivetrain); + public Auto auto; // #146: Pass in RobotContainer public IntakeSubsystem intakeSubsystem; public ElevatorSubsystem elevatorSubsystem; public ClimberSubsystem climberSubsystem; @@ -127,9 +131,8 @@ public RobotContainer() { stateManager = new SuperstructureStateManager(elevatorSubsystem, armSubsystem); + auto = new Auto(drivetrain, this); configureBindings(); - - drivetrain.setUpPathPlanner(); // Establish the "Trajectory Field" Field2d into the dashboard } @@ -326,11 +329,20 @@ private void configureBindings() { stateManager.setLastScoringPose( drivetrain.findNearestAprilTagPose()))); - stateManager.LEFT_CORAL.and(leftDriveController.getTrigger()).whileTrue(alignToReef(-0.2)); + stateManager + .LEFT_CORAL + .and(leftDriveController.getTrigger()) + .whileTrue(alignToReef(AligningConstants.leftOffset)); - stateManager.ALGAE.and(leftDriveController.getTrigger()).whileTrue(alignToReef(0)); + stateManager + .ALGAE + .and(leftDriveController.getTrigger()) + .whileTrue(alignToReef(AligningConstants.centerOffset)); - stateManager.RIGHT_CORAL.and(leftDriveController.getTrigger()).whileTrue(alignToReef(0.2)); + stateManager + .RIGHT_CORAL + .and(leftDriveController.getTrigger()) + .whileTrue(alignToReef(AligningConstants.rightOffset)); // Technical Bindings @@ -368,12 +380,20 @@ public Command getAutonomousCommand() { } public Command alignToReef(int tag, double offset) { - Pose2d alignmentPose = VisionConstants.aprilTagLayout.getTagPose(tag).get().toPose2d(); + Pose2d alignmentPose = + VisionConstants.aprilTagLayout + .getTagPose(tag) + .get() + .toPose2d() + .plus( + new Transform2d( + new Translation2d(Units.feetToMeters(3) / 2, offset), + new Rotation2d())); return new AlignToReef( drivetrain, leftJoystickVelocityX, leftJoystickVelocityY, - offset, + 0, alignmentPose, Rotation2d.kPi); // Skibidi } @@ -382,20 +402,38 @@ public Command alignToReef(int tag, double offset) { public Command alignToReef(double offset) { return Commands.defer( () -> { + Pose2d alignmentPose = + stateManager + .getLastScoringPose() + .plus( + new Transform2d( + new Translation2d( + Units.feetToMeters(3) / 2, offset), + new Rotation2d())); + // return new AlignAndDriveToReef(drivetrain, 0, alignmentPose, + // Rotation2d.kPi); return new AlignToReef( drivetrain, leftJoystickVelocityX, leftJoystickVelocityY, - offset, - stateManager.getLastScoringPose(), + 0, + alignmentPose, Rotation2d.kPi); }, Set.of(drivetrain)); } public Command alignAndDriveToReef(int tag, double offset) { - Pose2d alignmentPose = VisionConstants.aprilTagLayout.getTagPose(tag).get().toPose2d(); - return new AlignAndDriveToReef(drivetrain, offset, alignmentPose, Rotation2d.kPi); + Pose2d alignmentPose = + VisionConstants.aprilTagLayout + .getTagPose(tag) + .get() + .toPose2d() + .plus( + new Transform2d( + new Translation2d(Units.feetToMeters(3) / 2, offset), + new Rotation2d())); + return new AlignAndDriveToReef(drivetrain, 0, alignmentPose, Rotation2d.kPi); } public Command alignToPiece() { diff --git a/src/main/java/frc/robot/commands/AlignAndDriveToReef.java b/src/main/java/frc/robot/commands/AlignAndDriveToReef.java index be180911..011547fc 100644 --- a/src/main/java/frc/robot/commands/AlignAndDriveToReef.java +++ b/src/main/java/frc/robot/commands/AlignAndDriveToReef.java @@ -12,9 +12,9 @@ public class AlignAndDriveToReef extends Command { private CommandSwerveDrivetrain drivetrain; - private PIDController thetaController = new PIDController(4, 0, 0); - private PIDController yController = new PIDController(6, 0, 0); - private PIDController xController = new PIDController(3, 0, 0); + private PIDController thetaController = new PIDController(6, 0, 0); + private PIDController yController = new PIDController(8, 0, 0); + private PIDController xController = new PIDController(5, 0, 0); private Pose2d targetPose; private double offset; private Rotation2d rotationOffset; @@ -40,7 +40,7 @@ public void initialize() { thetaController.setSetpoint(rotationOffset.getRadians()); yController.setSetpoint(offset); thetaController.enableContinuousInput(0, 2 * Math.PI); - thetaController.setTolerance(Units.degreesToRadians(0.5)); + thetaController.setTolerance(Units.degreesToRadians(1)); yController.setTolerance(Units.inchesToMeters(0.4)); xController.setTolerance(Units.inchesToMeters(0.4)); } diff --git a/src/main/java/frc/robot/constants/AligningConstants.java b/src/main/java/frc/robot/constants/AligningConstants.java index 300264cc..3807cb45 100644 --- a/src/main/java/frc/robot/constants/AligningConstants.java +++ b/src/main/java/frc/robot/constants/AligningConstants.java @@ -7,4 +7,8 @@ public class AligningConstants { public static final double Kp = 0.9; public static final double Ki = 0.0; public static final double Kd = 0.0; + + public static final double leftOffset = -0.2; + public static final double rightOffset = 0.2; + public static final double centerOffset = 0; } diff --git a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java index cb4d2f69..5845e2cf 100644 --- a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java +++ b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java @@ -29,13 +29,13 @@ public class SuperstructureStateManager extends SubsystemBase { public class SuperstructureState { @FunctionalInterface - private interface StateChecker { + public interface StateChecker { boolean isAtTarget(Position position, SuperstructureStateManager stateManager); } - private static final StateChecker FALSE = (p, s) -> false; - private static final StateChecker TRUE = (p, s) -> true; - private static final StateChecker DEFAULT = + public static final StateChecker FALSE = (p, s) -> false; + public static final StateChecker TRUE = (p, s) -> true; + public static final StateChecker DEFAULT = (p, s) -> { // return (s.internalPosition == p); double armPosition = s.ArmSubsystem.getArmPosition(); @@ -49,6 +49,7 @@ private interface StateChecker { } else return false; }; + public static final StateChecker AUTO = DEFAULT; public enum Position { Sussy(1, 1, 1, null), @@ -339,5 +340,7 @@ public SuperstructureStateManager(ElevatorSubsystem elevatorheight, ArmSubsystem root.append(m_elevator); SmartDashboard.putData("Mech2d", mech); + + setDefaultCommand(moveToPosition(Position.Home)); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 8ae682f5..f16a7afc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -172,8 +172,6 @@ private SwerveState(String label) { private SwerveSetpointGenerator setpointGenerator; private SwerveSetpoint previousSetpoint; - public void setUpPathPlanner() {} - public Pose2d getRobotPose() { return getState().Pose; }