diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6a93e9ab..02fccf7c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -61,7 +61,7 @@ public void robotPeriodic() { @Override public void disabledInit() { - m_robotContainer.stopTagAlign(); + m_robotContainer.stopTagAlignAndPathHandler(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9b9abab..b7f9d492 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.algae.EjectAlgaeCommand; import frc.robot.commands.algae.IntakeAlgaeCommand; import frc.robot.commands.algae.OpenLoopAlgaeCommand; import frc.robot.commands.algae.ProcessorAlgaeCommand; @@ -361,10 +362,13 @@ private void configureDriverBindings() { new JoystickButton(driveJoystick, Button.M_SWE.id) .onTrue( + // new ScoreAlgaeCommand( + // robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); new ForceBargeCommand( - robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); - // new ForceBargeCommand( - // robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); // for easy + robotStateSubsystem, + elevatorSubsystem, + biscuitSubsystem, + algaeSubsystem)); // for easy // swapping Command floorAlgaeCommand = // new FloorAlgaeCommand( @@ -439,29 +443,32 @@ private void configureOperatorBindings() { new Trigger(() -> robotStateSubsystem.isStuckAndMisaligned()) .onTrue(new OperatorRumbleCommand(robotStateSubsystem, xboxController)); - // Move biscuit - new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) - .onTrue( - new JogBiscuitCommand( - biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) - .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); - new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) - .onTrue( - new JogBiscuitCommand( - biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) - .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); - - // Move elevator - new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) - .onTrue( - new JogElevatorCommand( - elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) - .onFalse(new HoldElevatorCommand(elevatorSubsystem)); - new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) - .onTrue( - new JogElevatorCommand( - elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) - .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + new Trigger(() -> xboxController.getPOV() != -1).onTrue(new EjectAlgaeCommand(algaeSubsystem)); + + // // Move biscuit + // new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) + // .onTrue( + // new JogBiscuitCommand( + // biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) + // .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + // new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) + // .onTrue( + // new JogBiscuitCommand( + // biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) + // .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + + // // Move elevator + // new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) + // .onTrue( + // new JogElevatorCommand( + // elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) + // .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + // new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) + // .onTrue( + // new JogElevatorCommand( + // elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, + // Rotations))) + // .onFalse(new HoldElevatorCommand(elevatorSubsystem)); } private void configureTestOperatorBindings() { @@ -801,8 +808,9 @@ public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } - public void stopTagAlign() { + public void stopTagAlignAndPathHandler() { tagAlignSubsystem.terminate(); + pathHandler.killPathHandler(); } public void setIsAuto(boolean isAuto) { @@ -830,8 +838,8 @@ public void updateCanivoreStatus() { } public void updateCANErrorCount() { - rCanErrors = RobotController.getCANStatus().receiveErrorCount; - tCanErrors = RobotController.getCANStatus().transmitErrorCount; + rCanErrors += RobotController.getCANStatus().receiveErrorCount; + tCanErrors += RobotController.getCANStatus().transmitErrorCount; } public void disableNoMotionCal() { diff --git a/src/main/java/frc/robot/commands/algae/EjectAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/EjectAlgaeCommand.java new file mode 100644 index 00000000..676abcb5 --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/EjectAlgaeCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; + +public class EjectAlgaeCommand extends InstantCommand { + private AlgaeSubsystem algaeSubsystem; + + public EjectAlgaeCommand(AlgaeSubsystem algaeSubsystem) { + this.algaeSubsystem = algaeSubsystem; + addRequirements(algaeSubsystem); + } + + @Override + public void initialize() { + algaeSubsystem.scoreProcessor(); + } +} diff --git a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java index 6e342000..c8ab8231 100644 --- a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java +++ b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java @@ -153,7 +153,7 @@ public boolean isFinished() { return true; } return (timer.hasElapsed(trajectory.getTotalTime() + AutonConstants.kAutoTimeout) - || (FastMath.sqrtQuick( + || (Math.sqrt( FastMath.pow2(driveSubsystem.getPoseMeters().getX() - finalPose.getX()) + FastMath.pow2((driveSubsystem.getPoseMeters().getY() - finalPose.getY()))) < AutonConstants.kMaxPathErrorMeters) diff --git a/src/main/java/frc/robot/commands/robotState/EjectAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/EjectAlgaeCommand.java new file mode 100644 index 00000000..a2b6b56d --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/EjectAlgaeCommand.java @@ -0,0 +1,21 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class EjectAlgaeCommand extends InstantCommand { + private AlgaeSubsystem algaeSubsystem; + private RobotStateSubsystem robotStateSubsystem; + + public EjectAlgaeCommand(AlgaeSubsystem algaeSubsystem, RobotStateSubsystem robotStateSubsystem) { + this.algaeSubsystem = algaeSubsystem; + this.robotStateSubsystem = robotStateSubsystem; + + addRequirements(algaeSubsystem); + } + + public void initialize() { + robotStateSubsystem.releaseAlgae(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java b/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java index 0d4b3237..07f5a771 100644 --- a/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java @@ -28,6 +28,7 @@ public ForceBargeCommand( @Override public void initialize() { + hasEjectedToBarge = false; startState = robotStateSubsystem.getState(); startingElevatorFinished = elevatorSubsystem.isFinished(); robotStateSubsystem.setAlgaeHeight(AlgaeHeight.HIGH); @@ -38,16 +39,21 @@ public void initialize() { @Override public boolean isFinished() { - if (startState == RobotStates.PROCESSOR_ALGAE || hasEjectedToBarge) { + if (startState == RobotStates.PROCESSOR_ALGAE + || startState == RobotStates.BARGE_ALGAE + || hasEjectedToBarge) { return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD || robotStateSubsystem.getState() == RobotStates.STOW || !startingElevatorFinished; - } else if (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE) { + } else if (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE + && robotStateSubsystem.getIsAutoPlacing()) { hasEjectedToBarge = true; + return false; } else { return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE + || (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE + && !robotStateSubsystem.getIsAutoPlacing()) || !robotStateSubsystem.getIsBargeSafe(); } - return false; } } diff --git a/src/main/java/frc/robot/commands/robotState/ForceProcessorCommand.java b/src/main/java/frc/robot/commands/robotState/ForceProcessorCommand.java index 17cc173d..a13581c3 100644 --- a/src/main/java/frc/robot/commands/robotState/ForceProcessorCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ForceProcessorCommand.java @@ -28,6 +28,7 @@ public ForceProcessorCommand( @Override public void initialize() { + hasEjectedToBarge = false; startState = robotStateSubsystem.getState(); startingElevatorFinished = elevatorSubsystem.isFinished(); robotStateSubsystem.setAlgaeHeight(AlgaeHeight.LOW); diff --git a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java index b84a4ea6..1d8253d9 100644 --- a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java @@ -26,9 +26,10 @@ public ScoreAlgaeCommand( @Override public void initialize() { + hasEjectedToBarge = false; startState = robotStateSubsystem.getState(); startingElevatorFinished = elevatorSubsystem.isFinished(); - if (startState == RobotStates.PROCESSOR_ALGAE) { + if (startState == RobotStates.PROCESSOR_ALGAE || startState == RobotStates.BARGE_ALGAE) { robotStateSubsystem.releaseAlgae(); } else { robotStateSubsystem.toScoreAlgae(); @@ -37,14 +38,19 @@ public void initialize() { @Override public boolean isFinished() { - if (startState == RobotStates.PROCESSOR_ALGAE || hasEjectedToBarge) { + if (startState == RobotStates.PROCESSOR_ALGAE + || startState == RobotStates.BARGE_ALGAE + || hasEjectedToBarge) { return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD || robotStateSubsystem.getState() == RobotStates.STOW || !startingElevatorFinished; - } else if (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE) { + } else if (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE + && robotStateSubsystem.getIsAutoPlacing()) { hasEjectedToBarge = true; } else { return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE + || (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE + && !robotStateSubsystem.getIsAutoPlacing()) || !robotStateSubsystem.getIsBargeSafe(); } return false; diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java index 056d2111..86f64282 100644 --- a/src/main/java/frc/robot/constants/AlgaeConstants.java +++ b/src/main/java/frc/robot/constants/AlgaeConstants.java @@ -32,7 +32,7 @@ public class AlgaeConstants { public static final double kIntakingSpeed = -1; public static final double kHasAlgaeVelThreshold = 10; - public static final double kSuperCycleHasAlgaeVelThres = 50; // was 40 + public static final double kSuperCycleHasAlgaeVelThres = 40; // was 40 public static final int kHasAlgaeCounts = 2; public static final double kHasCoralVelThreshold = 70; // FIXME diff --git a/src/main/java/frc/robot/constants/BargeAlignConstants.java b/src/main/java/frc/robot/constants/BargeAlignConstants.java index 58795922..07bc5f35 100644 --- a/src/main/java/frc/robot/constants/BargeAlignConstants.java +++ b/src/main/java/frc/robot/constants/BargeAlignConstants.java @@ -7,10 +7,8 @@ public class BargeAlignConstants { public static final double kBlueEjectAlgaeX = 7.67; // 7.72 public static final double kRedEjectAlgaeX = 9.86; // 9.81 - public static final double kBlueRaiseElevatorX = 7.22; // 7.22 - public static final double kRedRaiseElevatorX = 10.71; // 10.31 - public static final double kBlueUnsafeX = 7.02; - public static final double kRedUnsafeX = 10.51; + public static final double kBlueRaiseElevatorX = 7.02; // 7.22 + public static final double kRedRaiseElevatorX = 10.51; // 10.31 public static final Rotation2d kBlueDesiredYaw = Rotation2d.fromDegrees(0.0); public static final Rotation2d kRedDesiredYaw = Rotation2d.fromDegrees(180.0); diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index a038e60a..cdff32c4 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -63,7 +63,7 @@ public class ClimbConstants { public static final double kCloseEnoughX = 0.1; public static final double kCloseEnoughY = 0.03; public static final double kClimbAngleGood = RobotStateConstants.kClimbAngleSmall; - public static final double kFastClimbAfter = 3.5; + public static final double kFastClimbAt = 4.01; public static TalonFXConfiguration getPivotArmFxConfig() { TalonFXConfiguration armFxConfig = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index cc151c07..0d872176 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -8,9 +8,8 @@ public class RobotStateConstants { public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0}; public static final double kAlgaeRetreatDistance = 0; - // Use Barge align constants - // public static final double kBlueBargeSafeX = 7; // 7.6 - // public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX; + public static final double kBlueBargeSafeX = 7; // 7.6 + public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX; public static final double kCoralEjectTimer = 0.25; public static final double kAlgaeEjectTimer = 0.5; diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 2a5d548d..b4c422ed 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -41,7 +41,7 @@ public class TagServoingConstants { public static final int[] kBlueTargetTag = {18, 17, 22, 21, 20, 19}; public static final int[] kRedTargetTag = {7, 8, 9, 10, 11, 6}; - // Offsets + // Offsets (left is negative, right is pos) public static final double[][][] kBlueCoralOffset = { {{0, 0}, {0, 0}, {0, 0}, {0, 0}}, {{0, 0}, {0, 0}, {0, 0}, {0, 0}}, @@ -83,9 +83,9 @@ public class TagServoingConstants { // End conditions public static final double kInitialCloseEnough = 0.1; - public static final double kCoralDriveXCloseEnough = 0.03; + public static final double kCoralDriveXCloseEnough = 0.04; // was 0.03 public static final double kCoralDriveYCloseEnough = 0.02; // was 0.025 - public static final double kAlgaeDriveXCloseEnough = 0.03; + public static final double kAlgaeDriveXCloseEnough = 0.04; // was 0.03 public static final double kAlgaeDriveYCloseEnough = kCoralDriveYCloseEnough; public static final double kSmallYThres = 0.3; // For elevator staging diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java index 7a79a346..42c66175 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java @@ -7,6 +7,7 @@ public interface AlgaeIO { @AutoLog public static class AlgaeIOInputs { public double velocity; + public double statorCurrent; public boolean isBeamBroken; public boolean isCoralBeamBroken; } diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java index 692f4643..4d2abce3 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.signals.ForwardLimitValue; import com.ctre.phoenix6.signals.ReverseLimitValue; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import frc.robot.constants.AlgaeConstants; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -37,6 +38,7 @@ public class AlgaeIOFX implements AlgaeIO, Checkable { private StatusSignal revLimitSwitch; private VelocityVoltage speedRequest = new VelocityVoltage(0).withEnableFOC(false).withSlot(0); private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0).withEnableFOC(false); + private StatusSignal statorCurrent; public AlgaeIOFX() { logger = LoggerFactory.getLogger(this.getClass()); @@ -49,14 +51,16 @@ public AlgaeIOFX() { fwdLimitSwitch = talonFXS.getForwardLimit(); revLimitSwitch = talonFXS.getReverseLimit(); curVelocity = talonFXS.getVelocity(); + statorCurrent = talonFXS.getStatorCurrent(); } @Override public void updateInputs(AlgaeIOInputs inputs) { - BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch); + BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch, statorCurrent); inputs.velocity = curVelocity.getValueAsDouble(); inputs.isBeamBroken = fwdLimitSwitch.getValue().value == 0; inputs.isCoralBeamBroken = revLimitSwitch.getValue().value == 1; + inputs.statorCurrent = statorCurrent.getValueAsDouble(); } @Override diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java index 059ffdc6..a4de080c 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java @@ -63,7 +63,7 @@ public void scoreCoral() { public void holdAlgae() { // setSpeed(AlgaeConstants.kHoldSpeed); - setPct(0.06); + setPct(0.08); } public void holdCoral() { diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index 5bbae3d9..1e2987d8 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -102,7 +102,7 @@ public void prepClimb() { public void climb() { if (curState == ClimbState.PREPPED) { // setPosition(ClimbConstants.kClimbCagePos); - if (DriverStation.getMatchTime() < ClimbConstants.kFastClimbAfter) { + if (DriverStation.getMatchTime() <= ClimbConstants.kFastClimbAt) { Logger.recordOutput("Climb/isDesparate", true); io.setPercent(ClimbConstants.kClimbOpenLoopFastSpeed); spedUp = true; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 35472d86..300e2649 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -304,7 +304,7 @@ public boolean isDriveStill() { double vY = cs.vyMetersPerSecond; // Take fieldRel Speed and get the magnitude of the vector - double wheelSpeed = FastMath.sqrtQuick(FastMath.pow2(vX) + FastMath.pow2(vY)); + double wheelSpeed = Math.sqrt(FastMath.pow2(vX) + FastMath.pow2(vY)); boolean velStill = Math.abs(wheelSpeed) <= DriveConstants.kSpeedStillThreshold; boolean gyroStill = isGyroStill(); diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index f5e99b51..29dcf6bb 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -172,7 +172,7 @@ public boolean getCurrentLimiting() { } private void buildBase() { - + base = LEDPattern.solid(Color.kBlack); algae = LEDPattern.steps( Map.of( diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index 683408a8..5d78e586 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -219,7 +219,7 @@ private void drivePath() { driveSubsystem.calculateController( mirrorToProcessor(currPath.sampleAt(pathTimer.get(), mirrorTrajectory).get())); if (pathTimer.hasElapsed(currPath.getTotalTime() + AutonConstants.kAutoTimeout) - || (FastMath.sqrtQuick( + || (Math.sqrt( FastMath.pow2( driveSubsystem.getPoseMeters().getX() - currPathFinalPose.getX()) + FastMath.pow2( @@ -331,6 +331,7 @@ public void killPathHandler() { isHandling = false; curState = PathStates.DONE; runningPath = false; + robotStateSubsystem.setLEDLoadCoral(false); robotStateSubsystem.setIsAutoPlacing(true); pathTimer.stop(); pathTimer.reset(); @@ -451,7 +452,7 @@ public void periodic() { startPath(nextPath()); } double fetchDistance = - FastMath.sqrtQuick( + Math.sqrt( FastMath.pow2(driveSubsystem.getPoseMeters().getX() - currPathFinalPose.getX()) + FastMath.pow2( driveSubsystem.getPoseMeters().getY() - currPathFinalPose.getY())); diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index b41b2967..04eabcf5 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -9,7 +9,6 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; -import frc.robot.constants.BargeAlignConstants; import frc.robot.constants.DriveConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.RobotConstants; @@ -353,6 +352,13 @@ public void toStow() { driveSubsystem.removeDriveMultiplier(); driveSubsystem.setIgnoreSticks(false); + if (tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DONE) { + tagAlignSubsystem.terminate(); + } + if (bargeAlignSubsystem.getState() != BargeAlignSubsystem.BargeAlignStates.FINISHED) { + bargeAlignSubsystem.terminate(); + } + if (biscuitSubsystem.isSafeToStow()) { setBiscuitTransfer(RobotConstants.kStowSetpoint, true); elevatorSubsystem.setPosition(RobotConstants.kElevatorStowSetpoint); @@ -374,6 +380,13 @@ public void toStowSafe() { driveSubsystem.removeDriveMultiplier(); driveSubsystem.setIgnoreSticks(false); + if (tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DONE) { + tagAlignSubsystem.terminate(); + } + if (bargeAlignSubsystem.getState() != BargeAlignSubsystem.BargeAlignStates.FINISHED) { + bargeAlignSubsystem.terminate(); + } + if (algaeSubsystem.hasCoral()) { algaeSubsystem.holdCoral(); } else { @@ -388,6 +401,13 @@ public void toStowSequential() { setBiscuitTransfer(RobotConstants.kStowSetpoint, true); driveSubsystem.removeDriveMultiplier(); driveSubsystem.setIgnoreSticks(false); + + if (tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DONE) { + tagAlignSubsystem.terminate(); + } + if (bargeAlignSubsystem.getState() != BargeAlignSubsystem.BargeAlignStates.FINISHED) { + bargeAlignSubsystem.terminate(); + } if (algaeSubsystem.hasCoral()) { algaeSubsystem.holdCoral(); } else { @@ -475,8 +495,10 @@ private void toReefAlign(boolean getAlgae, boolean drive) { return; } boolean wantAlgae = (getAlgae || !hasCoral()) && scoringLevel != ScoringLevel.L1; - if ((hasAlgae() && scoreSide == ScoreSide.RIGHT && drive) || (hasAlgae() && !hasCoral())) { - setState(RobotStates.STOW); + if ( + /*(hasAlgae() && scoreSide == ScoreSide.RIGHT && drive) ||*/ (hasAlgae() && !hasCoral())) { + toStow(); + return; } if (drive) { @@ -673,8 +695,8 @@ public void toScoreAlgae() { } double poseX = driveSubsystem.getPoseMeters().getX(); isBargeSafe = - poseX > BargeAlignConstants.kRedRaiseElevatorX - || poseX < BargeAlignConstants.kBlueRaiseElevatorX; + poseX > RobotStateConstants.kRedBargeSafeX + || poseX < RobotStateConstants.kBlueBargeSafeX; if (isBargeSafe) { driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier); if (isAutoPlacing) { @@ -895,7 +917,7 @@ public void periodic() { if (algaeSubsystem.hasAlgaeSuperCycle()) { switch (getAlgaeLevel()) { case L2 -> { - biscuitSubsystem.setIsRemovingAlgae(true); + // biscuitSubsystem.setIsRemovingAlgae(true); biscuitSubsystem.setPosition( RobotConstants.kL2AlgaeRemovalSetpoint, true); // not using setBiscuitTransfer() to ensure hasAlgae is true @@ -985,7 +1007,8 @@ public void periodic() { case REMOVE_ALGAE -> { if (elevatorSubsystem.isFinished() && biscuitSubsystem.getPosition().in(Rotations) - <= (getAlgaeLevel() == ScoringLevel.L3 + <= ( + /*getAlgaeLevel() == ScoringLevel.L3*/ true ? RobotStateConstants.kBiscuitSuperCycleSafeThres : RobotStateConstants.kBiscuitSuperCycleSafeFastThres)) { biscuitSubsystem.setIsRemovingAlgae(false); @@ -1089,7 +1112,7 @@ public void periodic() { && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer)) { Pose2d currentPose = driveSubsystem.getPoseMeters(); double distanceFromRelease = - FastMath.sqrtQuick( + Math.sqrt( FastMath.pow2(currentPose.getX() - processorReleasePose.getX()) + FastMath.pow2(currentPose.getY() - processorReleasePose.getY())); Logger.recordOutput("RobotState/Processor Release Distance", distanceFromRelease); diff --git a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java index 5304003b..ace2ea09 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java @@ -5,13 +5,13 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.constants.BargeAlignConstants; import frc.robot.constants.DriveConstants; +import frc.robot.constants.RobotStateConstants; import frc.robot.controllers.FlyskyJoystick; import frc.robot.subsystems.drive.DriveSubsystem; import java.util.Set; import org.littletonrobotics.junction.Logger; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; -// TODO Make final drive faster. Reverse if we are too close to the barge. public class BargeAlignSubsystem extends MeasurableSubsystem { @@ -76,24 +76,15 @@ private void setupBargeAlign() { private boolean isSafe() { double poseX = driveSubsystem.getPoseMeters().getX(); - return poseX > BargeAlignConstants.kRedRaiseElevatorX - || poseX < BargeAlignConstants.kBlueRaiseElevatorX; + return poseX > RobotStateConstants.kRedBargeSafeX + || poseX < RobotStateConstants.kBlueBargeSafeX; } private boolean shouldRaiseElevator() { double poseX = driveSubsystem.getPoseMeters().getX(); return isOnBlueSide ? poseX > BargeAlignConstants.kBlueRaiseElevatorX - && poseX <= BargeAlignConstants.kBlueUnsafeX - : poseX < BargeAlignConstants.kRedRaiseElevatorX - && poseX >= BargeAlignConstants.kRedUnsafeX; - } - - private boolean shouldDriveBackwards() { - double poseX = driveSubsystem.getPoseMeters().getX(); - return isOnBlueSide - ? poseX > BargeAlignConstants.kBlueUnsafeX - : poseX < BargeAlignConstants.kRedUnsafeX; + : poseX < BargeAlignConstants.kRedRaiseElevatorX; } private boolean shouldEjectAlgae() { @@ -140,8 +131,6 @@ public void periodic() { Logger.recordOutput("BargeAlign/Vomega", vOmega); if (shouldRaiseElevator()) { setState(BargeAlignStates.RAISE_ELEV); - } else if (shouldDriveBackwards()) { - setState(BargeAlignStates.REVERSE); } } case RAISE_ELEV -> { @@ -156,15 +145,6 @@ public void periodic() { terminate(); } } - case REVERSE -> { - if (isOnBlueSide) { - driveSubsystem.move( - -vX, getYStickReading(), BargeAlignConstants.kBlueRaiseElevatorX, true); - //TODO add should raise elevator - } else { - driveSubsystem.move(vX, getYStickReading(), BargeAlignConstants.kRedRaiseElevatorX, true); - } - } case FINISHED -> {} } } @@ -178,7 +158,6 @@ public enum BargeAlignStates { // INIT, DRIVE, RAISE_ELEV, - REVERSE, FINISHED } } diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index c41f1e93..e4e57245 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -187,8 +187,7 @@ public double getCurRadius() { Translation2d reefRelative = driveSubsystem.getPoseMeters().getTranslation().minus(reefT); - return FastMath.sqrtQuick( - FastMath.pow2(reefRelative.getX()) + FastMath.pow2(reefRelative.getY())); + return Math.sqrt(FastMath.pow2(reefRelative.getX()) + FastMath.pow2(reefRelative.getY())); } public TagAlignStates getState() { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 79702f7e..7f7f5a2c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -186,7 +186,7 @@ private double minTagDistance(WallEyePoseResult result) { Pose3d tagPose = field.getTagPose(id).get(); double dist = - FastMath.sqrtQuick( + Math.sqrt( FastMath.pow2(tagPose.getX() - camLoc.getX()) + FastMath.pow2(tagPose.getY() - camLoc.getY())); @@ -208,7 +208,7 @@ private double averageTagDistance(WallEyePoseResult result) { for (int id : ids) { Pose3d tagPose = field.getTagPose(id).get(); totalDistance += - FastMath.sqrtQuick( + Math.sqrt( FastMath.pow2(tagPose.getX() - camLoc.getX()) + FastMath.pow2(tagPose.getY() - camLoc.getY())); } @@ -224,11 +224,9 @@ private boolean camsAgreeWithWheels(Translation3d pose, WallEyeTagResult result) Translation2d disp = (curPose.getTranslation().minus(pose.toTranslation2d())); double velMagnitude = - FastMath.sqrtQuick( - FastMath.pow2(vel.vxMetersPerSecond) + FastMath.pow2(vel.vyMetersPerSecond)); + Math.sqrt(FastMath.pow2(vel.vxMetersPerSecond) + FastMath.pow2(vel.vyMetersPerSecond)); - double dispMagnitude = - FastMath.sqrtQuick(FastMath.pow2(disp.getX()) + FastMath.pow2(disp.getY())); + double dispMagnitude = Math.sqrt(FastMath.pow2(disp.getX()) + FastMath.pow2(disp.getY())); /*This gets our displacement and compares it to who much we could have moved.It does this by getting the velocity and plotting it on a @@ -256,17 +254,15 @@ private double getStdDevFactor(double distance, int numTags, String camName) { if (numTags == 1) return 1 / (VisionConstants.FOV58YUYVBaseTrust - * FastMath.powQuick( - VisionConstants.baseNumber, - FastMath.powQuick( + * FastMath.exp( + FastMath.pow( VisionConstants.FOV58YUYVSingleTagCoeff * distance, VisionConstants.FOV58YUYVPowerNumber))); return 1 / (VisionConstants.FOV58YUYVBaseTrust - * FastMath.powQuick( - VisionConstants.baseNumber, - FastMath.powQuick( + * FastMath.exp( + FastMath.pow( VisionConstants.FOV58YUYVMultiTagCoeff * distance, VisionConstants.FOV58YUYVPowerNumber))); } @@ -274,17 +270,15 @@ private double getStdDevFactor(double distance, int numTags, String camName) { if (numTags == 1) return 1 / (VisionConstants.FOV58YUYVBaseTrust - * FastMath.powQuick( - VisionConstants.baseNumber, - FastMath.powQuick( + * FastMath.exp( + FastMath.pow( VisionConstants.FOV58MJPGSingleTagCoeff * distance, VisionConstants.FOV58YUYVPowerNumber))); return 1 / (VisionConstants.FOV58YUYVBaseTrust - * FastMath.powQuick( - VisionConstants.baseNumber, - FastMath.powQuick( + * FastMath.exp( + FastMath.pow( VisionConstants.FOV58MJPGSingleTagCoeff * distance, VisionConstants.FOV58YUYVPowerNumber))); } @@ -293,16 +287,14 @@ private double getStdDevFactor(double distance, int numTags, String camName) { if (numTags == 1) return 1 / (VisionConstants.FOV75YUYVBaseTrust - * FastMath.powQuick( - VisionConstants.baseNumber, - FastMath.powQuick( + * FastMath.exp( + FastMath.pow( VisionConstants.FOV75YUYVSingleTagCoeff * distance, VisionConstants.FOV75YUYVPowerNumber))); return 1 / (VisionConstants.FOV75YUYVBaseTrust - * FastMath.powQuick( - VisionConstants.baseNumber, - FastMath.powQuick( + * FastMath.exp( + FastMath.pow( VisionConstants.FOV75YUYVMultiTagCoeff * distance, VisionConstants.FOV75YUYVPowerNumber))); } @@ -311,15 +303,13 @@ private double getStdDevFactor(double distance, int numTags, String camName) { if (numTags == 1) return 1 / (VisionConstants.baseTrust - * FastMath.powQuick( - VisionConstants.baseNumber, - FastMath.powQuick( + * FastMath.exp( + FastMath.pow( VisionConstants.singleTagCoeff * distance, VisionConstants.powerNumber))); return 1 / (VisionConstants.baseTrust - * FastMath.powQuick( - VisionConstants.baseNumber, - FastMath.powQuick( + * FastMath.exp( + FastMath.pow( VisionConstants.multiTagCoeff * distance, VisionConstants.powerNumber))); } }