diff --git a/.vscode/settings.json b/.vscode/settings.json index d2f7034..ef24bd2 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,6 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable", + "wpilib.autoStartRioLog": false } diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..00784de 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ 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/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3c8f0a4..6e73085 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -106,7 +106,7 @@ public final class Constants { public static final boolean DRIFT_CORRECTION_ENABLED = true; public static final boolean INVERT_X = false; - public static final boolean INVERT_Y = false; + public static final boolean INVERT_Y = true; public static final boolean INVERT_ROTATION = false; // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); @@ -121,8 +121,8 @@ public final class Constants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.25); - private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.5); + private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); @@ -234,15 +234,18 @@ public final class Constants { public static final Gains XY_GAINS = new Gains(3,0.01,0.0); public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); - public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0); + public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); + public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); + public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0); + public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, 1, 0); public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole - public static final int LIDAR_DIO_CHANNEL = 2; + public static final int LIDAR_DIO_CHANNEL = 7; public static final int LIDAR_MICROS_TO_CM = 10; public static final int SECONDS_TO_MICROS = 1000000; public static final double XY_TOLERANCE = 0.07; // Meters - public static final double ROT_TOLERANCE = 1; // Degrees + public static final double ROT_TOLERANCE = 5; // Degrees // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); // public static final Pose2d targetpos = @@ -343,8 +346,8 @@ public final class Constants { public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT"; public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; - public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); - public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); + public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); + public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters @@ -358,11 +361,21 @@ public final class Constants { public static final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(9.5); - public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); - ;;;;;;;;;;;;;;;;;;;;;;;;;;;; + + public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); + // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right - public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17); + public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); + public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); + + public static final double L2_ALGAE_REMOVAL = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(3); + + + public static final double L2_SCORE_DISTANCE = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(1); @@ -389,7 +402,8 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; - public static final int XBOX_PROGRAMMER_ID = 2; + public static final int BUTTONBOX_ID = 2; + public static final int XBOX_PROGRAMMER_ID = 3; public static final double LEFT_AXIS_DEADBAND = 0.1; } @@ -398,20 +412,31 @@ public final class Constants { public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); - public static final int BASIN_LIMIT_SWITCH = 0; // TODO: FIND - public static final int ENDEFFECTOR_LIMIT_SWITCH = 1; // TODO: FIND + public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND + public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND + + public static final double GEAR_RATIO_ELEVATOR = -9.0; + //Max for elevator = 50% - public static final double GEAR_RATIO_ELEVATOR = 36.0; - public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; - public static final double WAITING_POSITION_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find 4-6 off the ground - public static final double MAX_POSITION_ELEVATOR = 20 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position + public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe + public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe + public static final double SCORING_THREE_ELEVATOR = -9.25; + public static final double DEALGAE_L2_ELEVATOR = -23.5; + public static final double DEALGAE_L3_ELEVATOR = -33.75; + public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position + + public static final double GEAR_RATIO_ENDEFECTOR = -100.0; + public static final double ENDEFECTOR_DRIVE_SLOW = -0.08; + //Max for endefector = 60% - public static final double GEAR_RATIO_ENDEFECTOR = 100.0; + public static final double L2_SCORE_ENDEFECTOR = -19; public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; + public static final double DEALGAE_L2_EENDEFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; - public static final double SCORING_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value + public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; + public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3297915..00e4f50 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,11 +16,14 @@ import java.util.List; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.utility.controller.XboxController; +import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.robot.Constants.FieldConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; @@ -40,6 +43,8 @@ import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.waitElevatorRefrence; +import frc4388.robot.commands.waitEndefectorRefrence; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -85,7 +90,8 @@ public class RobotContainer { /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID); /* Virtual Controllers */ @@ -109,25 +115,114 @@ public class RobotContainer { // () -> autoplaybackName.get(), // lastAutoName // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, // true, false); - private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); - private Command AprilLidarAlign = new SequentialCommandGroup( - new GotoLastApril(m_robotSwerveDrive, m_vision), - new InstantCommand(() -> System.out.println("Soup")), - new WaitCommand(1), - new LidarAlign(m_robotSwerveDrive, m_lidar) - ); - private Command AprilLidarLeft = new SequentialCommandGroup( - AutoGotoPosition.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_lidar) + // 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 waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + 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 AprilLidarRight = new SequentialCommandGroup( - AutoGotoPosition.asProxy(), - new InstantCommand(() -> System.out.println("Soup")), - new WaitCommand(1), - new LidarAlign(m_robotSwerveDrive, m_lidar)//, + 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, 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 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")), + // new WaitCommand(1), + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT), + new LidarAlign(m_robotSwerveDrive, m_lidar), + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), // new MoveForTimeCommand(m_robotSwerveDrive, - // new Translation2d(0, 0.5), new Translation2d(), 1000, true) + // new Translation2d(0,1), new Translation2d(), 500, true), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + ); + + 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), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT), + + new LidarAlign(m_robotSwerveDrive, m_lidar), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(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 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), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(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 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), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(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 placeCoral = new SequentialCommandGroup( @@ -147,15 +242,52 @@ public class RobotContainer { new WaitCommand(2), new InstantCommand(() -> System.out.println("Done yoinking some coral...")) ); + + 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), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT), + new LidarAlign(m_robotSwerveDrive, m_lidar), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, 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 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), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT), + new LidarAlign(m_robotSwerveDrive, m_lidar), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), 500, true), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + ); + + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition); - NamedCommands.registerCommand("april-allign", AprilLidarAlign); - 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(); @@ -175,6 +307,12 @@ public class RobotContainer { }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); m_robotSwerveDrive.setToSlow(); + // m_robotElevator.setDefaultCommand(new RunCommand(() -> { + // m_robotElevator.driveElevatorStick(m_operatorXbox.getRight()); + // }, m_robotElevator) + // .withName("Elevator")); + + makeAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); // this.subsystems.add(m_robotSwerveDrive); @@ -220,54 +358,116 @@ public class RobotContainer { private void configureButtonBindings() { // ? /* Driver Buttons */ - DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); // ! /* Speed */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // final + new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) // final + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + - new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); + + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + + + + + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); // ? /* Operator Buttons */ - - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .onTrue(AutoGotoPosition); - .onTrue(AprilLidarRight); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)); - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator)); - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator)); - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator)); + new JoystickButton(getButtonBox(), ButtonBox.Five) + .onTrue(AprilLidarAlignL4Left); + new JoystickButton(getButtonBox(), ButtonBox.One) + .onTrue(AprilLidarAlignL4Right); + + new JoystickButton(getButtonBox(), ButtonBox.Six) + .onTrue(AprilLidarAlignL3Left); + + new JoystickButton(getButtonBox(), ButtonBox.Two) + .onTrue(AprilLidarAlignL3Right); + + new JoystickButton(getButtonBox(), ButtonBox.Seven) + .onTrue(AprilLidarAlignL2Left); + + new JoystickButton(getButtonBox(), ButtonBox.Three) + .onTrue(AprilLidarAlignL2Right); + + + // Lower coral removal + new JoystickButton(getButtonBox(), ButtonBox.Eight) + .onTrue(leftAlgaeRemove); + + // Upper coral removal + 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(() -> getDeadbandedOperatorController().getPOV() == 0) + .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) + .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_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)*/ @@ -320,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); @@ -373,6 +575,10 @@ public class RobotContainer { return this.m_operatorXbox; } + public ButtonBox getButtonBox() { + return this.m_buttonBox; + } + public VirtualController getVirtualDriverController() { return m_virtualDriver; } diff --git a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt deleted file mode 100644 index c99ee2c..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt +++ /dev/null @@ -1,225 +0,0 @@ -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,12 -0.0,0.0,0.0,0.0,26 -0.0,0.0,0.0,0.0,37 -0.0,0.0,0.0,0.0,50 -0.0,0.0,0.0,0.0,62 -0.0,0.0,0.0,0.0,73 -0.0,0.0,0.0,0.0,88 -0.0,0.0,0.0,0.0,103 -0.0,0.0,0.0,0.0,116 -0.0,0.0,0.0,0.0,160 -0.0,0.0,0.0,0.0,173 -0.0,0.0,0.0,0.0,185 -0.0,0.0,0.0,0.0,200 -0.0,0.0,0.0,0.0,211 -0.0,0.0,0.0,0.0,223 -0.0,0.0,0.0,0.0,235 -0.0,0.0,0.0,0.0,247 -0.0,0.0,0.0,0.0,263 -0.0,0.0,0.0,0.0,283 -0.0,0.0,0.0,0.0,303 -0.0,-0.109375,0.0,0.0,323 -0.0,-0.1484375,0.0,0.0,343 -0.0,-0.2109375,0.0,0.0,363 -0.0,-0.3671875,0.0,0.0,398 -0.0,-0.4140625,0.0,0.0,411 -0.0,-0.4765625,0.0,0.0,425 -0.0,-0.5078125,0.0,0.0,443 -0.0,-0.5078125,0.0,0.0,463 -0.0,-0.53125,0.0,0.0,483 -0.0,-0.5546875,0.0,0.0,504 -0.0,-0.5625,0.0,0.0,523 -0.0,-0.5625,0.0,0.0,544 -0.0,-0.5703125,0.0,0.0,563 -0.0,-0.5859375,0.0,0.0,584 -0.0,-0.5859375,0.0,0.0,603 -0.0,-0.5859375,0.0,0.0,640 -0.0,-0.59375,0.0,0.0,657 -0.0,-0.6015625,0.0,0.0,672 -0.0,-0.6015625,0.0,0.0,685 -0.0,-0.6015625,0.0,0.0,703 -0.0,-0.6015625,0.0,0.0,723 -0.0,-0.6015625,0.0,0.0,743 -0.0,-0.6015625,0.0,0.0,763 -0.0,-0.6015625,0.0,0.0,783 -0.0,-0.6015625,0.0,0.0,803 -0.0,-0.6015625,0.0,0.0,823 -0.0,-0.6015625,0.0,0.0,844 -0.0,-0.6015625,0.0,0.0,878 -0.0,-0.6015625,0.0,0.0,893 -0.0,-0.6015625,0.0,0.0,907 -0.0,-0.6015625,0.0,0.0,924 -0.0,-0.609375,0.0,0.0,943 -0.0,-0.609375,0.0,0.0,963 -0.0,-0.609375,0.0,0.0,983 -0.0,-0.609375,0.0,0.0,1004 -0.0,-0.609375,0.0,0.0,1023 -0.0,-0.609375,0.0,0.0,1043 -0.0,-0.609375,0.0,0.0,1064 -0.0,-0.609375,0.0,0.0,1083 -0.0,-0.609375,0.0,0.0,1156 -0.0,-0.609375,0.0,0.0,1172 -0.0,-0.609375,0.0,0.0,1185 -0.0,-0.609375,0.0,0.0,1200 -0.0,-0.609375,0.0,0.0,1215 -0.0,-0.609375,0.0,0.0,1225 -0.0,-0.609375,0.0,0.0,1236 -0.0,-0.609375,0.0,0.0,1249 -0.0,-0.609375,0.0,0.0,1263 -0.0,-0.609375,0.0,0.0,1283 -0.0,-0.609375,0.0,0.0,1303 -0.0,-0.609375,0.0,0.0,1323 -0.0,-0.609375,0.0,0.0,1363 -0.0,-0.6015625,0.0,0.0,1376 -0.0,-0.6015625,0.0,0.0,1394 -0.0,-0.6015625,0.0,0.0,1405 -0.0,-0.6015625,0.0,0.0,1423 -0.0,-0.6015625,0.0,0.0,1443 -0.0,-0.6015625,0.0,0.0,1463 -0.0,-0.6015625,0.0,0.0,1483 -0.0,-0.6015625,0.0,0.0,1503 -0.0,-0.6015625,0.0,0.0,1523 -0.0,-0.6015625,0.0,0.0,1543 -0.0,-0.6015625,0.0,0.0,1563 -0.0,-0.6015625,0.0,0.0,1597 -0.0,-0.6015625,0.0,0.0,1608 -0.0,-0.6015625,0.0,0.0,1624 -0.0,-0.6015625,0.0,0.0,1643 -0.0,-0.6015625,0.0,0.0,1664 -0.0,-0.5859375,0.0,0.0,1683 -0.0,-0.5859375,0.0,0.0,1703 -0.0,-0.5625,0.0,0.0,1723 -0.0,-0.5625,0.0,0.0,1743 -0.0,-0.5625,0.0,0.0,1763 -0.0,-0.5625,0.0,0.0,1783 -0.0,-0.5625,0.0,0.0,1803 -0.0,-0.5625,0.0,0.0,1843 -0.0,-0.5625,0.0,0.0,1855 -0.0,-0.5625,0.0,0.0,1868 -0.0,-0.5625,0.0,0.0,1883 -0.0,-0.5625,0.0,0.0,1903 -0.0,-0.5625,0.0,0.0,1923 -0.0,-0.5625,0.0,0.0,1943 -0.0,-0.5625,0.0,0.0,1963 -0.0,-0.5625,0.0,0.0,1983 -0.0,-0.5625,0.0,0.0,2003 -0.0,-0.5625,0.0,0.0,2024 -0.0,-0.5625,0.0,0.0,2043 -0.0,-0.5625,0.0,0.0,2081 -0.0,-0.5625,0.0,0.0,2093 -0.0,-0.5625,0.0,0.0,2105 -0.0,-0.5625,0.0,0.0,2123 -0.0,-0.5625,0.0,0.0,2143 -0.0,-0.5625,0.0,0.0,2163 -0.0,-0.5625,0.0,0.0,2183 -0.0,-0.5625,0.0,0.0,2203 -0.0,-0.5625,0.0,0.0,2223 -0.0,-0.5625,0.0,0.0,2243 -0.0,-0.5625,0.0,0.0,2263 -0.0,-0.5625,0.0,0.0,2283 -0.0,-0.5625,0.0,0.0,2366 -0.0,-0.5625,0.0,0.0,2377 -0.0,-0.5625,0.0,0.0,2394 -0.0,-0.5703125,0.0,0.0,2405 -0.0,-0.5703125,0.0,0.0,2418 -0.0,-0.5703125,0.0,0.0,2431 -0.0,-0.5703125,0.0,0.0,2444 -0.0,-0.5703125,0.0,0.0,2458 -0.0,-0.5703125,0.0,0.0,2470 -0.0,-0.5703125,0.0,0.0,2485 -0.0,-0.5703125,0.0,0.0,2503 -0.0,-0.5703125,0.0,0.0,2523 -0.0,-0.5703125,0.0,0.0,2563 -0.0,-0.5703125,0.0,0.0,2577 -0.0,-0.5703125,0.0,0.0,2591 -0.0,-0.5703125,0.0,0.0,2608 -0.0,-0.5703125,0.0,0.0,2624 -0.0,-0.5703125,0.0,0.0,2643 -0.0,-0.5703125,0.0,0.0,2677 -0.0,-0.5703125,0.0,0.0,2698 -0.0,-0.5703125,0.0,0.0,2711 -0.0,-0.5703125,0.0,0.0,2725 -0.0,-0.5703125,0.0,0.0,2743 -0.0,-0.5703125,0.0,0.0,2764 -0.0,-0.5703125,0.0,0.0,2810 -0.0,-0.5703125,0.0,0.0,2820 -0.0,-0.5703125,0.0,0.0,2833 -0.0,-0.5703125,0.0,0.0,2845 -0.0,-0.5703125,0.0,0.0,2863 -0.0,-0.5703125,0.0,0.0,2883 -0.0,-0.5703125,0.0,0.0,2904 -0.0,-0.5703125,0.0,0.0,2924 -0.0,-0.5703125,0.0,0.0,2943 -0.0,-0.5703125,0.0,0.0,2963 -0.0,-0.5703125,0.0,0.0,2983 -0.0,-0.5703125,0.0,0.0,3003 -0.0,-0.5703125,0.0,0.0,3033 -0.0,-0.5703125,0.0,0.0,3050 -0.0,-0.5703125,0.0,0.0,3065 -0.0,-0.5703125,0.0,0.0,3083 -0.0,-0.5703125,0.0,0.0,3103 -0.0,-0.5703125,0.0,0.0,3123 -0.0,-0.5703125,0.0,0.0,3144 -0.0,-0.5703125,0.0,0.0,3164 -0.0,-0.5703125,0.0,0.0,3184 -0.0,-0.5703125,0.0,0.0,3203 -0.0,-0.5703125,0.0,0.0,3223 -0.0,-0.5703125,0.0,0.0,3243 -0.0,-0.5703125,0.0,0.0,3272 -0.0,-0.5703125,0.0,0.0,3289 -0.0,-0.5703125,0.0,0.0,3303 -0.0,-0.5703125,0.0,0.0,3323 -0.0,-0.5703125,0.0,0.0,3343 -0.0,-0.5703125,0.0,0.0,3363 -0.0,-0.5703125,0.0,0.0,3383 -0.0,-0.5703125,0.0,0.0,3403 -0.0,-0.5703125,0.0,0.0,3423 -0.0,-0.5703125,0.0,0.0,3443 -0.0,-0.5703125,0.0,0.0,3463 -0.0,-0.5703125,0.0,0.0,3483 -0.0,-0.5703125,0.0,0.0,3566 -0.0,-0.5703125,0.0,0.0,3578 -0.0,-0.5703125,0.0,0.0,3596 -0.0,-0.5703125,0.0,0.0,3610 -0.0,-0.5703125,0.0,0.0,3623 -0.0,-0.5703125,0.0,0.0,3640 -0.0,-0.5703125,0.0,0.0,3651 -0.0,-0.5703125,0.0,0.0,3663 -0.0,-0.5703125,0.0,0.0,3678 -0.0,-0.5703125,0.0,0.0,3691 -0.0,-0.5703125,0.0,0.0,3706 -0.0,-0.5703125,0.0,0.0,3723 -0.0,-0.5703125,0.0,0.0,3766 -0.0,-0.5703125,0.0,0.0,3778 -0.0,-0.5703125,0.0,0.0,3792 -0.0,-0.5703125,0.0,0.0,3807 -0.0,-0.5703125,0.0,0.0,3823 -0.0,-0.5703125,0.0,0.0,3843 -0.0,-0.53125,0.0,0.0,3863 -0.0,-0.53125,0.0,0.0,3884 -0.0,-0.421875,0.0,0.0,3904 -0.0,0.0,0.0,0.0,3924 -0.0,0.0,0.0,0.0,3944 -0.0,0.0,0.0,0.0,3963 -0.0,0.0,0.0,0.0,3999 -0.0,0.0,0.0,0.0,4010 -0.0,0.0,0.0,0.0,4025 -0.0,0.0,0.0,0.0,4043 -0.0,0.0,0.0,0.0,4063 -0.0,0.0,0.0,0.0,4083 -0.0,0.0,0.0,0.0,4103 -0.0,0.0,0.0,0.0,4123 -0.0,0.0,0.0,0.0,4143 -0.0,0.0,0.0,0.0,4163 -0.0,0.0,0.0,0.0,4183 -0.0,0.0,0.0,0.0,4203 -0.0,0.0,0.0,0.0,4236 -0.0,0.0,0.0,0.0,4247 -0.0,0.0,0.0,0.0,4264 -0.0,0.0,0.0,0.0,4284 -0.0,0.0,0.0,0.0,4304 -0.0,0.0,0.0,0.0,4324 -0.0,0.0,0.0,0.0,4343 -0.0,0.0,0.0,0.0,4363 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 60f87ac..b02c209 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -1,8 +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; @@ -28,15 +33,19 @@ public class GotoLastApril extends Command { SwerveDrive swerveDrive; Vision vision; + double distance; + Side side; /** * Command to drive robot to position. * @param SwerveDrive m_robotSwerveDrive */ - public GotoLastApril(SwerveDrive swerveDrive, Vision vision) { + public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) { this.swerveDrive = swerveDrive; this.vision = vision; + this.distance = distance; + this.side = side; // addRequirements(swerveDrive); } @@ -46,11 +55,18 @@ public class GotoLastApril extends Command { tagRelativeXError = val; } + Alliance alliance = null; + @Override public void initialize() { xPID.initialize(); yPID.initialize(); - this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.LEFT, AutoConstants.X_OFFSET_TRIM.get()); + 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; @@ -59,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); @@ -70,6 +96,12 @@ public class GotoLastApril extends Command { double youtput = yPID.update(yerr); double rotoutput = rotPID.update(roterr); + 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; + + + Translation2d leftStick = new Translation2d( Math.max(Math.min(-youtput, 1), -1), Math.max(Math.min(-xoutput, 1), -1) @@ -81,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/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index a40a878..e25c374 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -52,7 +52,7 @@ public class LidarAlign extends Command { return; } - if (currentFinderTick > 100) { //arbutrary threshhold for now. + if (currentFinderTick > 10) { //arbutrary threshhold for now. headedRight = !headedRight; currentFinderTick *= -1; } diff --git a/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java b/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java new file mode 100644 index 0000000..d6d803d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Elevator; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitElevatorRefrence extends Command { + /** Creates a new waitElevatorRefrence. */ + private Elevator elevator; + public waitElevatorRefrence(Elevator elevator) { + this.elevator = elevator; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevator.elevatorAtRefrence(); + } +} diff --git a/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java b/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java new file mode 100644 index 0000000..4a1a83e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Elevator; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitEndefectorRefrence extends Command { + /** Creates a new waitElevatorRefrence. */ + private Elevator elevator; + public waitEndefectorRefrence(Elevator elevator) { + this.elevator = elevator; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevator.endefectorAtRefrence(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 78c7588..0601c39 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -4,26 +4,43 @@ package frc4388.robot.subsystems; +import java.time.Instant; + import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ElevatorConstants; +import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ private TalonFX elevatorMotor; private TalonFX endefectorMotor; - private DigitalInput basinLimitSwitch; + private long wait = 0; + private long maxWait = 1000; + + private double elevatorRefrence = 0; + private double endefectorRefrence = 0; + + private DigitalInput basinBeamBreak; private DigitalInput endefectorLimitSwitch; public enum CoordinationState { - Waiting, // for coral into the though - Ready, // Has coral in enefector + Waiting, // for coral into the though + WatingBeamTriped, //once the beam break trips + Ready, // Has coral in endefector + Hovering, // Has coral in endefector + L2Score, PrimedThree, // Arm and elevator Waiting to score in the level 3 position ScoringThree, // Arm and elevator in the level three position PrimedFour, // Arm and elevator Waiting to score in the level 4 position @@ -40,7 +57,7 @@ public class Elevator extends SubsystemBase { elevatorMotor = elevatorTalonFX; endefectorMotor = endefectorTalonFX; - this.basinLimitSwitch = basinLimitSwitch; + this.basinBeamBreak = basinLimitSwitch; this.endefectorLimitSwitch = endefectorLimitSwitch; elevatorMotor.setNeutralMode(NeutralModeValue.Brake); @@ -54,8 +71,11 @@ public class Elevator extends SubsystemBase { //PID methods private void PIDPosition(TalonFX motor, double position) { - var request = new PositionVoltage(position); - elevatorMotor.setControl(request); + if (motor == elevatorMotor) elevatorRefrence = position; + else endefectorRefrence = position; + + var request = new PositionDutyCycle(position); + motor.setControl(request); } public void elevatorStop() { @@ -70,47 +90,157 @@ public class Elevator extends SubsystemBase { currentState = state; switch (currentState) { case Waiting: { + wait = System.currentTimeMillis() + maxWait; PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); break; } + case WatingBeamTriped: { + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + break; + } + case Ready: { PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); break; } - case ScoringThree: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); + case Hovering: { + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + break; + } + + case L2Score: { + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.L2_SCORE_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + break; + } + + case PrimedFour: { + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); break; } case ScoringFour: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); break; } + + case PrimedThree: { + PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR); + break; + } + + case ScoringThree: { + PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + break; + } + + case BallRemoverL2Primed: { + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR); + break; + } + + case BallRemoverL2Go: { + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + break; + } + + case BallRemoverL3Primed: { + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR); + break; + } + + case BallRemoverL3Go: { + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + break; + } + + default: { + assert false; + } } } - private void periodicWaiting() { - if (basinLimitSwitch.get()) transitionState(CoordinationState.Ready); + public boolean elevatorAtRefrence() { + // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); + double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); + boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.5; + if (atPos) { + SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence); + SmartDashboard.putNumber("Elevator Pos", elevatorPosition); + } + + return atPos; } + + public boolean endefectorAtRefrence() { + // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); + double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble(); + + return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2; + } + // public void driveElevatorStick(Translation2d stick) { + // if (stick.getNorm() > 0.05) { + // elevatorMotor.set(stick.getY()); + // } + // } + + private void periodicWaiting() { + if (!basinBeamBreak.get()) + transitionState(CoordinationState.Ready); + } + + // private void periodicWaitingTripped() { + // if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) + // transitionState(CoordinationState.Ready); + // } + private void periodicReady() { + if (elevatorAtRefrence()) + transitionState(CoordinationState.Hovering); + } + private void periodicScoring() { if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting); } @Override public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); + SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0); + SmartDashboard.putString("State", currentState.toString()); + if (currentState == CoordinationState.Waiting) { - periodicWaiting(); - } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { - periodicScoring(); + // periodicWaiting(); + } else if (currentState == CoordinationState.WatingBeamTriped) { + // periodicWaitingTripped(); + } else if (currentState == CoordinationState.Ready) { + periodicReady(); + } + // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { + // periodicScoring(); + // } + } + + public void armShuffle(){ + if(!basinBeamBreak.get()){ + //shuffle the coral with the arm until coral hits beam break } } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 9c2a1dc..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 @@ -304,8 +314,9 @@ public class SwerveDrive extends Subsystem { } public void stopModules() { - stopped = true; - swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + // stopped = true; + // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + softStop(); } @Override diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index 3890a20..d063568 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -12,7 +12,8 @@ import frc4388.robot.Constants.FieldConstants; public class ReefPositionHelper { public enum Side { LEFT, - RIGHT + RIGHT, + CENTER } public static final Pose2d[] RED_TAGS = { @@ -72,10 +73,22 @@ public class ReefPositionHelper { return new Pose2d(); } - public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim) { + public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) { return offset(getNearestTag(position), - (side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim, - FieldConstants.VERTICAL_SCORING_POSITION_OFFSET); + getSide(side) + xtrim, + ydistance); + } + + public static double getSide(Side side){ + switch(side) { + case LEFT: + return -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET); + case RIGHT: + return (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET); + case CENTER: + return 0; + } + return 0; } @@ -87,6 +100,6 @@ public class ReefPositionHelper { return new Pose2d(new Translation2d( oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset, oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset - ), oldPose.getRotation()); + ), oldPose.getRotation().rotateBy(Rotation2d.k180deg)); } } diff --git a/src/main/java/frc4388/utility/controller/ButtonBox.java b/src/main/java/frc4388/utility/controller/ButtonBox.java new file mode 100644 index 0000000..33c7744 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/ButtonBox.java @@ -0,0 +1,19 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; + +public class ButtonBox extends GenericHID { + public static final int White = 1; + public static final int One = 2; + public static final int Two = 3; + public static final int Three = 4; + public static final int Four = 5; + public static final int Five = 6; + public static final int Six = 7; + public static final int Seven = 8; + public static final int Eight = 9; + + public ButtonBox(int ID){ + super(ID); + } +}