diff --git a/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto b/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto new file mode 100644 index 0000000..9fbd56c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Blue Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "lineup-no-arm" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path index 88c93bc..321e817 100644 --- a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 7.270707070707072, - "y": 7.013257575757576 + "x": 6.599873737373737, + "y": 8.029671717171716 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.309027777777779, - "y": 5.3768308080808085 + "x": 5.464431818181818, + "y": 5.6403977272727275 }, "prevControl": { - "x": 4.945156854248459, - "y": 5.08761998976538 + "x": 5.240820707070706, + "y": 5.254160353535354 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 60.255118703057796 + "rotation": -122.99770510121631 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -2.223961240385466 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ba3194d..00e4f50 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -117,38 +117,48 @@ public class RobotContainer { // true, false); // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); private Command AprilLidarAlignL4Right = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), - // new InstantCommand(() -> System.out.println("Soup")), - // new WaitCommand(1), + new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT), + + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.RIGHT), new LidarAlign(m_robotSwerveDrive, m_lidar), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) ); private Command AprilLidarAlignL4Left = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), // new InstantCommand(() -> System.out.println("Soup")), // new WaitCommand(1), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT), + + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT), new LidarAlign(m_robotSwerveDrive, m_lidar), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), 500, true), + new Translation2d(0,1), new Translation2d(), 500, true), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) ); private Command AprilLidarAlignL3Left = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT), // new InstantCommand(() -> System.out.println("Soup")), @@ -166,6 +176,7 @@ public class RobotContainer { ); private Command AprilLidarAlignL3Right = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT), @@ -187,6 +198,7 @@ public class RobotContainer { ); private Command AprilLidarAlignL2Left = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -200,6 +212,7 @@ public class RobotContainer { ); private Command AprilLidarAlignL2Right = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -231,6 +244,7 @@ public class RobotContainer { ); private Command leftAlgaeRemove = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -243,6 +257,7 @@ public class RobotContainer { ); private Command rightAlgaeRemove = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -261,9 +276,18 @@ public class RobotContainer { */ public RobotContainer() { - NamedCommands.registerCommand("AutoGotoPosition", AprilLidarAlignL4Right); - NamedCommands.registerCommand("april-allign", AprilLidarAlignL4Right); - NamedCommands.registerCommand("place-coral", placeCoral); + NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); + NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); + NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL3Left); + NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL3Right); + NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL2Left); + NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL2Right); + + NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup( + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + new LidarAlign(m_robotSwerveDrive, m_lidar) + )); + NamedCommands.registerCommand("grab-coral", grabCoral); configureButtonBindings(); @@ -333,10 +357,6 @@ public class RobotContainer { */ private void configureButtonBindings() { - // ? /* Test button box bindings */ - new JoystickButton(getButtonBox(), ButtonBox.White) - .onTrue(new InstantCommand(() -> System.out.println("White!"))); - // ? /* Driver Buttons */ DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); @@ -360,18 +380,18 @@ public class RobotContainer { - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) - .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) - .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp())); - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) - .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) - .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown())); @@ -419,23 +439,35 @@ public class RobotContainer { new JoystickButton(getButtonBox(), ButtonBox.Four) .onTrue(rightAlgaeRemove); + + // Cancel button new JoystickButton(getButtonBox(), ButtonBox.White) .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator)); + // Score prep + // Prime 4 + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator)); + + // Prime 3 + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator)); + + + //Trims - new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) - .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp())); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) - .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) + .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown())); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) - .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp())); - - new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) - .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) + .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp())); + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) + .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown())); // ? /* Programer Buttons (Controller 3)*/ @@ -488,16 +520,18 @@ public class RobotContainer { */ public Command getAutonomousCommand() { + //return autoPlayback; //return new GotoPositionCommand(m_robotSwerveDrive, m_vision) //return autoChooser.getSelected(); // try{ - // // Load the path you want to follow using its name in the GUI - return autoCommand; + // // // Load the path you want to follow using its name in the GUI + // return autoCommand; // } catch (Exception e) { // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); - // return Commands.none(); + return Commands.none(); // } + // return new PathPlannerAuto("Line-up-no-arm"); // zach told me to do the below comment //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 7bd2861..b02c209 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -1,9 +1,13 @@ package frc4388.robot.commands; +import java.util.Optional; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; @@ -51,6 +55,8 @@ public class GotoLastApril extends Command { tagRelativeXError = val; } + Alliance alliance = null; + @Override public void initialize() { xPID.initialize(); @@ -58,6 +64,9 @@ public class GotoLastApril extends Command { this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())); + Optional a = DriverStation.getAlliance(); + if(!a.isEmpty()) + alliance = a.get(); } double xerr; @@ -66,8 +75,18 @@ public class GotoLastApril extends Command { @Override public void execute() { - xerr = targetpos.getX() - vision.getPose2d().getX(); - yerr = targetpos.getY() - vision.getPose2d().getY(); + + if (alliance == Alliance.Red) { + xerr = -(targetpos.getX() - vision.getPose2d().getX()); + yerr = (targetpos.getY() - vision.getPose2d().getY()); + } + else{ + + xerr = targetpos.getX() - vision.getPose2d().getX(); + yerr = targetpos.getY() - vision.getPose2d().getY(); + } + + roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); // SmartDashboard.putNumber("PID X Error", xerr); @@ -80,8 +99,8 @@ public class GotoLastApril extends Command { xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1; + - // System.out.println(xerr + ", " + yerr + ", " + roterr + ", " + rotoutput); Translation2d leftStick = new Translation2d( Math.max(Math.min(-youtput, 1), -1), @@ -94,6 +113,8 @@ public class GotoLastApril extends Command { 0 ); + + setTagRelativeXError( new Translation2d(xerr, yerr).getAngle() .rotateBy(targetpos.getRotation()) diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 5a3fe33..0601c39 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -227,7 +227,7 @@ public class Elevator extends SubsystemBase { SmartDashboard.putString("State", currentState.toString()); if (currentState == CoordinationState.Waiting) { - periodicWaiting(); + // periodicWaiting(); } else if (currentState == CoordinationState.WatingBeamTriped) { // periodicWaitingTripped(); } else if (currentState == CoordinationState.Ready) { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 20c41fe..1221930 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -141,10 +141,20 @@ public class SwerveDrive extends Subsystem { return; // don't bother doing swerve drive math and return early. leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - if (SwerveDriveConstants.INVERT_X) leftStick = new Translation2d(-leftStick.getX(), leftStick.getY()); - if (SwerveDriveConstants.INVERT_Y) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY()); + + Optional alliance = DriverStation.getAlliance(); + + if(!alliance.isEmpty()){ + if (alliance.get() == Alliance.Red) + leftStick = new Translation2d(-leftStick.getX(), leftStick.getY()); + else + leftStick = new Translation2d(leftStick.getX(), -leftStick.getY()); + // if (alliance.get() != Alliance.Red) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY()); + } if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1); + + + stopped = false; if (fieldRelative) { // ! drift correction