diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index cc7a8d3..26f3fdc 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -366,7 +366,7 @@ public final class Constants { // 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(17); + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); 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); @@ -419,7 +419,7 @@ public final class Constants { //Max for elevator = 50% public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; - public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe + public static final double WAITING_POSITION_ELEVATOR = -9.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; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 828ed46..2377656 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.CanDevice; import frc4388.utility.DeferredBlock; +import frc4388.utility.DeferredBlockMulti; import frc4388.utility.RobotTime; import frc4388.utility.Status; import frc4388.utility.Subsystem; @@ -113,6 +114,7 @@ public class Robot extends TimedRobot { @Override public void disabledExit() { DeferredBlock.execute(); + DeferredBlockMulti.execute(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 02c797c..4ce4eb5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.DriverStation; import java.io.File; import java.util.ArrayList; import java.util.List; +import java.util.function.BooleanSupplier; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; @@ -42,6 +43,7 @@ import edu.wpi.first.wpilibj2.command.Commands; // Autos import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.GotoLastApril; +import frc4388.robot.commands.IfCommand; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.waitElevatorRefrence; @@ -64,8 +66,10 @@ import frc4388.robot.subsystems.SwerveDrive; // Utilites import frc4388.utility.DeferredBlock; +import frc4388.utility.DeferredBlockMulti; import frc4388.utility.ReefPositionHelper; import frc4388.utility.Subsystem; +import frc4388.utility.TimesNegativeOne; import frc4388.utility.ReefPositionHelper.Side; import frc4388.utility.configurable.ConfigurableString; @@ -118,11 +122,15 @@ 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_robotSwerveDrive.startSlowPeriod();}), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT) + )), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), + // 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), @@ -137,35 +145,51 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); 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 InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT) + )), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + 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) + + + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); 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 InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT) + )), + 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), @@ -173,13 +197,19 @@ public class RobotContainer { 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) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL3Right = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT) + )), + 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), @@ -195,11 +225,12 @@ public class RobotContainer { 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) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL2Left = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -208,12 +239,13 @@ public class RobotContainer { 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) + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL2Right = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -222,8 +254,8 @@ public class RobotContainer { 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) - + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command placeCoral = new SequentialCommandGroup( @@ -245,7 +277,7 @@ public class RobotContainer { ); private Command leftAlgaeRemove = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -254,11 +286,12 @@ public class RobotContainer { 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) + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command rightAlgaeRemove = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -267,7 +300,8 @@ public class RobotContainer { 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) + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -293,7 +327,12 @@ public class RobotContainer { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); + new DeferredBlock(() -> { // Called on first robot enable + m_robotSwerveDrive.resetGyro(); + }); + new DeferredBlockMulti(() -> { // Called on every robot enable + TimesNegativeOne.update(); + }); DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); @@ -451,9 +490,13 @@ public class RobotContainer { .onTrue(rightAlgaeRemove); - // Cancel button - new JoystickButton(getButtonBox(), ButtonBox.White) - .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator)); + // Cancel button + new JoystickButton(getButtonBox(), ButtonBox.White) + .onTrue(new InstantCommand(() -> { + m_robotElevator.elevatorStop(); + m_robotElevator.endefectorStop(); + m_robotSwerveDrive.endSlowPeriod(); + }, m_robotElevator)); // Score prep // Prime 4 @@ -616,7 +659,7 @@ public class RobotContainer { // return autoCommand; // } catch (Exception e) { // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); - return Commands.none(); + return autoCommand; // } // return new PathPlannerAuto("Line-up-no-arm"); // zach told me to do the below comment @@ -638,9 +681,9 @@ public class RobotContainer { autoChooser.onChange((filename) -> { autoCommand = new PathPlannerAuto(filename); - System.out.println("Robot Auto Changed"); + System.out.println("Robot Auto Changed " + filename); }); - SmartDashboard.putData(autoChooser); + // SmartDashboard.putData(autoChooser); } diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index b02c209..b9fd1be 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -15,6 +15,7 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Vision; import frc4388.utility.Gains; import frc4388.utility.ReefPositionHelper; +import frc4388.utility.TimesNegativeOne; import frc4388.utility.ReefPositionHelper.Side; import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; import frc4388.utility.UtilityStructs.AutoRecordingFrame; @@ -75,19 +76,26 @@ public class GotoLastApril extends Command { @Override public void execute() { - - 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(); + xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); + yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis); + // xerr = targetpos.getX() - vision.getPose2d().getX(); + // yerr = targetpos.getX() - vision.getPose2d().getY(); + + // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed); + + roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); + + boolean invert = Math.abs(roterr) > 180; + + if(roterr > 180){ + roterr -= 360; + }else if(roterr < -180){ + roterr += 360; } - - roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); + SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); + SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); + SmartDashboard.putNumber("Rotational PID error", roterr); // SmartDashboard.putNumber("PID X Error", xerr); // SmartDashboard.putNumber("PID Y Error", yerr); diff --git a/src/main/java/frc4388/robot/commands/IfCommand.java b/src/main/java/frc4388/robot/commands/IfCommand.java new file mode 100644 index 0000000..0f5cd47 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/IfCommand.java @@ -0,0 +1,33 @@ +package frc4388.robot.commands; + +import java.time.Instant; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// Command that is called if something is true +public class IfCommand extends InstantCommand { + BooleanSupplier isTrue; + Command trueCommand; + Command falseCommand; + + public IfCommand(BooleanSupplier isTrue, Command trueCommand, Command falseCommand){ + this.isTrue = isTrue; + this.trueCommand = trueCommand; + this.falseCommand = falseCommand; + } + + public IfCommand(BooleanSupplier isTrue, Command trueCommand){ + this(isTrue, trueCommand, Commands.none()); + } + + @Override + public void execute() { + if(isTrue.getAsBoolean()) + trueCommand.schedule(); + else + falseCommand.schedule(); + } +} diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index e25c374..4ae9ad4 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -21,6 +21,7 @@ public class LidarAlign extends Command { private boolean foundReef; private boolean headedRight; private double speed; + private int bounces; // private final boolean constructedHeadedRight; /** Creates a new LidarAlign. */ @@ -40,6 +41,7 @@ public class LidarAlign extends Command { this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; this.headedRight = !(GotoLastApril.tagRelativeXError < 0); + this.bounces = 0; } @@ -55,6 +57,8 @@ public class LidarAlign extends Command { if (currentFinderTick > 10) { //arbutrary threshhold for now. headedRight = !headedRight; currentFinderTick *= -1; + bounces++; + if (bounces == 2) return; } double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI; double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef @@ -78,7 +82,10 @@ public class LidarAlign extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - if (foundReef && lidar.withinDistance()) { // spot on + if (lidar.getDistance() < 4) { + swerveDrive.stopModules(); + return true; + } else if (foundReef && lidar.withinDistance()) { // spot on swerveDrive.stopModules(); return true; } else if (foundReef && !lidar.withinDistance()) { // over shot @@ -87,6 +94,9 @@ public class LidarAlign extends Command { currentFinderTick = 0; foundReef = false; return false; + } else if (bounces == 2) { + swerveDrive.stopModules(); + return true; } else { return false; } diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java index 88166a5..fdbb619 100644 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java @@ -5,6 +5,7 @@ import java.time.Instant; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.TimesNegativeOne; // Command to repeat a joystick movement for a specific time. public class MoveForTimeCommand extends Command { diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index eb0366d..f58dbee 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -181,20 +181,26 @@ public class Elevator extends SubsystemBase { public boolean elevatorAtReference() { // 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); - } + double diffrence = elevatorRefrence - elevatorPosition; - return atPos; + boolean headedUp = diffrence < 0; + boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; + boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; + + return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); } public boolean endeffectorAtReference() { // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); + double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble(); + double diffrence = endefectorRefrence - endefectorPosition; - return Math.abs(endeffectorRefrence - endeffectorPosition) <= 0.2; + boolean headedUp = diffrence < 0; + boolean forwardLimit = endefectorMotor.getForwardLimit().asSupplier().get().value == 0; + boolean reverseLimit = endefectorMotor.getReverseLimit().asSupplier().get().value == 0; + + return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); } // public void driveElevatorStick(Translation2d stick) { // if (stick.getNorm() > 0.05) { @@ -261,6 +267,14 @@ public class Elevator extends SubsystemBase { // } } + public boolean isL4Primed(){ + return currentState == CoordinationState.PrimedFour; + } + + public boolean isL3Primed(){ + return currentState == CoordinationState.PrimedThree; + } + 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/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 130efc3..5fad561 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -72,7 +72,7 @@ public class Lidar extends Subsystem { if(distance == -1){ s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED"); }else{ - s.addReport(ReportLevel.ERROR, "LIDAR CONNECTED"); + s.addReport(ReportLevel.INFO, "LIDAR CONNECTED"); } return s; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1221930..eff4e3c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -35,9 +35,8 @@ import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.utility.Status; import frc4388.utility.Subsystem; +import frc4388.utility.TimesNegativeOne; import frc4388.utility.Status.ReportLevel; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.config.PIDConstants; @@ -103,11 +102,11 @@ public class SwerveDrive extends Subsystem { // This will flip the path being followed to the red side of the field. // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; + // var alliance = DriverStation.getAlliance(); + // if (alliance.isPresent()) { + // return alliance.get() == DriverStation.Alliance.Red; + // } + return TimesNegativeOne.isRed; }, this // Reference to this subsystem to set requirements ); @@ -140,23 +139,14 @@ public class SwerveDrive extends Subsystem { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput return; // don't bother doing swerve drive math and return early. - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - 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); - - + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); stopped = false; if (fieldRelative) { + + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); + // ! drift correction if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) { rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); @@ -218,7 +208,7 @@ public class SwerveDrive extends Subsystem { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput return; // don't bother doing swerve drive math and return early. - leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) @@ -282,7 +272,7 @@ public class SwerveDrive extends Subsystem { // if (leftStick.getNorm() < 0.05) //if no imput // return; // don't bother doing swerve drive math and return early. - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * -speedAdjust) @@ -390,6 +380,18 @@ public class SwerveDrive extends Subsystem { rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; } + private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR; + + public void startSlowPeriod() { + tmp_gear_index = gear_index; + setToSlow(); + } + + public void endSlowPeriod() { + setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); + gear_index = tmp_gear_index; + } + @Override public String getSubsystemName() { return "Swerve Drive Controller"; diff --git a/src/main/java/frc4388/utility/Alliance.java b/src/main/java/frc4388/utility/Alliance.java new file mode 100644 index 0000000..0f96914 --- /dev/null +++ b/src/main/java/frc4388/utility/Alliance.java @@ -0,0 +1,5 @@ +package frc4388.utility; + +public class Alliance { + +} diff --git a/src/main/java/frc4388/utility/DeferredBlockMulti.java b/src/main/java/frc4388/utility/DeferredBlockMulti.java new file mode 100644 index 0000000..9e8b284 --- /dev/null +++ b/src/main/java/frc4388/utility/DeferredBlockMulti.java @@ -0,0 +1,18 @@ +package frc4388.utility; + +import java.util.ArrayList; + +public class DeferredBlockMulti { + private static ArrayList m_blocks = new ArrayList<>(); + + public DeferredBlockMulti(Runnable block) { + m_blocks.add(block); + } + + public static void execute() { + + for (Runnable block : m_blocks) { + block.run(); + } + } +} diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index d063568..e8cb0f2 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -63,14 +63,11 @@ public class ReefPositionHelper { * Function to find closest tag location based on side */ public static Pose2d getNearestTag(Pose2d position) { - Optional ally = DriverStation.getAlliance(); - if (!ally.isPresent()) - return new Pose2d(); - if (ally.get() == Alliance.Red) + + if(TimesNegativeOne.isRed) return getNearestTag(RED_TAGS, position); - if (ally.get() == Alliance.Blue) - return getNearestTag(BLUE_TAGS, position); - return new Pose2d(); + else + return getNearestTag(BLUE_TAGS, position); } public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) { diff --git a/src/main/java/frc4388/utility/TimesNegativeOne.java b/src/main/java/frc4388/utility/TimesNegativeOne.java new file mode 100644 index 0000000..bf9818f --- /dev/null +++ b/src/main/java/frc4388/utility/TimesNegativeOne.java @@ -0,0 +1,51 @@ +package frc4388.utility; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.SwerveDriveConstants; + +// Class that holds weather the drivers sticks should be inverted +public class TimesNegativeOne { + + public static boolean XAxis = SwerveDriveConstants.INVERT_X; + public static boolean YAxis = SwerveDriveConstants.INVERT_Y; + public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION; + public static boolean isRed = false; + public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET); + + private static boolean isRed() { + Optional alliance = DriverStation.getAlliance(); + if(alliance.isEmpty()) return false; + return (alliance.get() == Alliance.Red); + } + + public static void update(){ + isRed = isRed(); + XAxis = SwerveDriveConstants.INVERT_X ^ isRed; + YAxis = SwerveDriveConstants.INVERT_Y ^ isRed; + RotAxis = SwerveDriveConstants.INVERT_ROTATION; + ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); + SmartDashboard.putBoolean("Is red alliance", isRed); + } + + public static double invert(double num, boolean invert){ + return invert ? -num : num; + } + + public static Translation2d invert(Translation2d stick, boolean invertXY){ + if(invertXY) return stick.times(-1); + else return stick; + } + + public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){ + return new Translation2d( + invert(stick.getX(), invertX), + invert(stick.getY(), invertY) + ); + } +} diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.2.4.json similarity index 87% rename from vendordeps/PathplannerLib-2025.1.1.json rename to vendordeps/PathplannerLib-2025.2.4.json index 6322388..24add57 100644 --- a/vendordeps/PathplannerLib-2025.1.1.json +++ b/vendordeps/PathplannerLib-2025.2.4.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.1.1.json", + "fileName": "PathplannerLib-2025.2.4.json", "name": "PathplannerLib", - "version": "2025.1.1", + "version": "2025.2.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.1.1" + "version": "2025.2.4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,