diff --git a/Robot_Bubbles/config.json b/Robot_Bubbles/config.json index 6ea429a..7db3326 100644 --- a/Robot_Bubbles/config.json +++ b/Robot_Bubbles/config.json @@ -55,6 +55,38 @@ 0.156, -0.405 ] + },{ + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + 0.2861912, + -0.0635, + -0.104775 + ] + },{ + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + 0.2861912, + -0.0635, + -0.104775 + ] } ] } \ No newline at end of file diff --git a/Robot_Bubbles/model_2.glb b/Robot_Bubbles/model_2.glb new file mode 100644 index 0000000..99dc190 Binary files /dev/null and b/Robot_Bubbles/model_2.glb differ diff --git a/Robot_Bubbles/model_3.glb b/Robot_Bubbles/model_3.glb new file mode 100644 index 0000000..d723ab8 Binary files /dev/null and b/Robot_Bubbles/model_3.glb differ diff --git a/simgui-ds.json b/simgui-ds.json index f178b77..f430671 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -95,6 +95,7 @@ "useGamepad": true }, { + "guid": "78696e70757401000000000000000000", "useGamepad": true } ] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 93a7e0e..ef84cd8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -72,10 +72,19 @@ public static class SimConstants { } public static class SOTMConstants { - public static InterpolatingTreeMap hoodAngleMap = + public static InterpolatingTreeMap hoodAngleMapScoring = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); - public static InterpolatingDoubleTreeMap shooterSpeedMap = new InterpolatingDoubleTreeMap(); - public static InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); + public static InterpolatingDoubleTreeMap shooterSpeedMapScoring = + new InterpolatingDoubleTreeMap(); + public static InterpolatingDoubleTreeMap timeOfFlightMapScoring = + new InterpolatingDoubleTreeMap(); + + public static InterpolatingTreeMap hoodAngleMapFerrying = + new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); + public static InterpolatingDoubleTreeMap shooterSpeedMapFerrying = + new InterpolatingDoubleTreeMap(); + public static InterpolatingDoubleTreeMap timeOfFlightMapFerrying = + new InterpolatingDoubleTreeMap(); // static { // hoodAngleMap.put(1.623, Rotation2d.fromDegrees(21.448)); @@ -107,59 +116,114 @@ public static class SOTMConstants { // } static { - hoodAngleMap.put(1.681, Rotation2d.fromDegrees(21.448)); - hoodAngleMap.put(2.016, Rotation2d.fromDegrees(21.972)); - hoodAngleMap.put(2.338, Rotation2d.fromDegrees(22.648)); - hoodAngleMap.put(2.665, Rotation2d.fromDegrees(24.122)); - hoodAngleMap.put(3.034, Rotation2d.fromDegrees(25.658)); - hoodAngleMap.put(3.363, Rotation2d.fromDegrees(26.333)); - hoodAngleMap.put(3.711, Rotation2d.fromDegrees(27.562)); - hoodAngleMap.put(4.061, Rotation2d.fromDegrees(29.036)); - hoodAngleMap.put(4.324, Rotation2d.fromDegrees(29.835)); - hoodAngleMap.put(4.619, Rotation2d.fromDegrees(30.449)); - hoodAngleMap.put(5.010, Rotation2d.fromDegrees(32.046)); - hoodAngleMap.put(5.366, Rotation2d.fromDegrees(33.275)); - hoodAngleMap.put(5.610, Rotation2d.fromDegrees(34.319)); - hoodAngleMap.put(6.051, Rotation2d.fromDegrees(35.794)); - hoodAngleMap.put(6.369, Rotation2d.fromDegrees(36.408)); - hoodAngleMap.put(6.704, Rotation2d.fromDegrees(37.002)); - hoodAngleMap.put(7.022, Rotation2d.fromDegrees(36.531)); - - shooterSpeedMap.put(1.681, 5.795); - shooterSpeedMap.put(2.016, 5.940); - shooterSpeedMap.put(2.338, 6.229); - shooterSpeedMap.put(2.665, 6.422); - shooterSpeedMap.put(3.034, 6.711); - shooterSpeedMap.put(3.363, 6.903); - shooterSpeedMap.put(3.711, 7.096); - shooterSpeedMap.put(4.061, 7.337); - shooterSpeedMap.put(4.324, 7.529); - shooterSpeedMap.put(4.619, 7.674); - shooterSpeedMap.put(5.010, 7.915); - shooterSpeedMap.put(5.366, 8.156); - shooterSpeedMap.put(5.610, 8.252); - shooterSpeedMap.put(6.051, 8.445); - shooterSpeedMap.put(6.369, 8.637); - shooterSpeedMap.put(6.704, 8.830); - shooterSpeedMap.put(7.022, 9.023); - - timeOfFlightMap.put(1.695, 0.893); - timeOfFlightMap.put(2.016, 0.924); - timeOfFlightMap.put(2.338, 0.985); - timeOfFlightMap.put(2.665, 1.013); - timeOfFlightMap.put(3.034, 1.059); - timeOfFlightMap.put(3.363, 1.093); - timeOfFlightMap.put(3.711, 1.118); - timeOfFlightMap.put(4.061, 1.147); - timeOfFlightMap.put(4.324, 1.175); - timeOfFlightMap.put(4.619, 1.195); - timeOfFlightMap.put(5.010, 1.216); - timeOfFlightMap.put(5.366, 1.242); - timeOfFlightMap.put(5.610, 1.241); - timeOfFlightMap.put(6.051, 1.249); - timeOfFlightMap.put(6.365, 1.273); - timeOfFlightMap.put(6.704, 1.295); - timeOfFlightMap.put(7.022, 1.341); + hoodAngleMapScoring.put(1.681, Rotation2d.fromDegrees(21.448)); + hoodAngleMapScoring.put(2.016, Rotation2d.fromDegrees(21.972)); + hoodAngleMapScoring.put(2.338, Rotation2d.fromDegrees(22.648)); + hoodAngleMapScoring.put(2.665, Rotation2d.fromDegrees(24.122)); + hoodAngleMapScoring.put(3.034, Rotation2d.fromDegrees(25.658)); + hoodAngleMapScoring.put(3.363, Rotation2d.fromDegrees(26.333)); + hoodAngleMapScoring.put(3.711, Rotation2d.fromDegrees(27.562)); + hoodAngleMapScoring.put(4.061, Rotation2d.fromDegrees(29.036)); + hoodAngleMapScoring.put(4.324, Rotation2d.fromDegrees(29.835)); + hoodAngleMapScoring.put(4.619, Rotation2d.fromDegrees(30.449)); + hoodAngleMapScoring.put(5.010, Rotation2d.fromDegrees(32.046)); + hoodAngleMapScoring.put(5.366, Rotation2d.fromDegrees(33.275)); + hoodAngleMapScoring.put(5.610, Rotation2d.fromDegrees(34.319)); + hoodAngleMapScoring.put(6.051, Rotation2d.fromDegrees(35.794)); + hoodAngleMapScoring.put(6.369, Rotation2d.fromDegrees(36.408)); + hoodAngleMapScoring.put(6.704, Rotation2d.fromDegrees(37.002)); + hoodAngleMapScoring.put(7.022, Rotation2d.fromDegrees(36.531)); + + shooterSpeedMapScoring.put(1.681, 5.795); + shooterSpeedMapScoring.put(2.016, 5.940); + shooterSpeedMapScoring.put(2.338, 6.229); + shooterSpeedMapScoring.put(2.665, 6.422); + shooterSpeedMapScoring.put(3.034, 6.711); + shooterSpeedMapScoring.put(3.363, 6.903); + shooterSpeedMapScoring.put(3.711, 7.096); + shooterSpeedMapScoring.put(4.061, 7.337); + shooterSpeedMapScoring.put(4.324, 7.529); + shooterSpeedMapScoring.put(4.619, 7.674); + shooterSpeedMapScoring.put(5.010, 7.915); + shooterSpeedMapScoring.put(5.366, 8.156); + shooterSpeedMapScoring.put(5.610, 8.252); + shooterSpeedMapScoring.put(6.051, 8.445); + shooterSpeedMapScoring.put(6.369, 8.637); + shooterSpeedMapScoring.put(6.704, 8.830); + shooterSpeedMapScoring.put(7.022, 9.023); + + timeOfFlightMapScoring.put(1.695, 0.893); + timeOfFlightMapScoring.put(2.016, 0.924); + timeOfFlightMapScoring.put(2.338, 0.985); + timeOfFlightMapScoring.put(2.665, 1.013); + timeOfFlightMapScoring.put(3.034, 1.059); + timeOfFlightMapScoring.put(3.363, 1.093); + timeOfFlightMapScoring.put(3.711, 1.118); + timeOfFlightMapScoring.put(4.061, 1.147); + timeOfFlightMapScoring.put(4.324, 1.175); + timeOfFlightMapScoring.put(4.619, 1.195); + timeOfFlightMapScoring.put(5.010, 1.216); + timeOfFlightMapScoring.put(5.366, 1.242); + timeOfFlightMapScoring.put(5.610, 1.241); + timeOfFlightMapScoring.put(6.051, 1.249); + timeOfFlightMapScoring.put(6.365, 1.273); + timeOfFlightMapScoring.put(6.704, 1.295); + timeOfFlightMapScoring.put(7.022, 1.341); + + // ferrying maps + hoodAngleMapFerrying.put(1.681, Rotation2d.fromDegrees(22.448)); + hoodAngleMapFerrying.put(2.016, Rotation2d.fromDegrees(22.972)); + hoodAngleMapFerrying.put(2.338, Rotation2d.fromDegrees(23.648)); + hoodAngleMapFerrying.put(2.665, Rotation2d.fromDegrees(25.122)); + hoodAngleMapFerrying.put(3.034, Rotation2d.fromDegrees(26.658)); + hoodAngleMapFerrying.put(3.363, Rotation2d.fromDegrees(27.333)); + hoodAngleMapFerrying.put(3.711, Rotation2d.fromDegrees(28.562)); + hoodAngleMapFerrying.put(4.061, Rotation2d.fromDegrees(30.036)); + hoodAngleMapFerrying.put(4.324, Rotation2d.fromDegrees(30.835)); + hoodAngleMapFerrying.put(4.619, Rotation2d.fromDegrees(31.449)); + hoodAngleMapFerrying.put(5.010, Rotation2d.fromDegrees(33.046)); + hoodAngleMapFerrying.put(5.366, Rotation2d.fromDegrees(34.275)); + hoodAngleMapFerrying.put(5.610, Rotation2d.fromDegrees(35.319)); + hoodAngleMapFerrying.put(6.051, Rotation2d.fromDegrees(36.794)); + hoodAngleMapFerrying.put(6.369, Rotation2d.fromDegrees(37.408)); + hoodAngleMapFerrying.put(6.704, Rotation2d.fromDegrees(38.002)); + hoodAngleMapFerrying.put(7.022, Rotation2d.fromDegrees(37.531)); + + shooterSpeedMapFerrying.put(1.681, 5.595); + shooterSpeedMapFerrying.put(2.016, 5.740); + shooterSpeedMapFerrying.put(2.338, 6.029); + shooterSpeedMapFerrying.put(2.665, 6.222); + shooterSpeedMapFerrying.put(3.034, 6.511); + shooterSpeedMapFerrying.put(3.363, 6.703); + shooterSpeedMapFerrying.put(3.711, 6.896); + shooterSpeedMapFerrying.put(4.061, 7.137); + shooterSpeedMapFerrying.put(4.324, 7.329); + shooterSpeedMapFerrying.put(4.619, 7.474); + shooterSpeedMapFerrying.put(5.010, 7.715); + shooterSpeedMapFerrying.put(5.366, 7.956); + shooterSpeedMapFerrying.put(5.610, 8.052); + shooterSpeedMapFerrying.put(6.051, 8.245); + shooterSpeedMapFerrying.put(6.369, 8.437); + shooterSpeedMapFerrying.put(6.704, 8.630); + shooterSpeedMapFerrying.put(7.022, 8.823); + + timeOfFlightMapFerrying.put(1.695, 0.893 * 1.5); + timeOfFlightMapFerrying.put(2.016, 0.924 * 1.5); + timeOfFlightMapFerrying.put(2.338, 0.985 * 1.5); + timeOfFlightMapFerrying.put(2.665, 1.013 * 1.5); + timeOfFlightMapFerrying.put(3.034, 1.059 * 1.5); + timeOfFlightMapFerrying.put(3.363, 1.093 * 1.5); + timeOfFlightMapFerrying.put(3.711, 1.118 * 1.5); + timeOfFlightMapFerrying.put(4.061, 1.147 * 1.5); + timeOfFlightMapFerrying.put(4.324, 1.175 * 1.5); + timeOfFlightMapFerrying.put(4.619, 1.195 * 1.5); + timeOfFlightMapFerrying.put(5.010, 1.216 * 1.5); + timeOfFlightMapFerrying.put(5.366, 1.242 * 1.5); + timeOfFlightMapFerrying.put(5.610, 1.241 * 1.5); + timeOfFlightMapFerrying.put(6.051, 1.249 * 1.5); + timeOfFlightMapFerrying.put(6.365, 1.273 * 1.5); + timeOfFlightMapFerrying.put(6.704, 1.295 * 1.5); + timeOfFlightMapFerrying.put(7.022, 1.341 * 1.5); } } @@ -191,13 +255,57 @@ public static class SwerveConstants { public static final double steerKA = 0.0; } + public static class ClimbConstants { + public static final Pose2d rightClimbPose = + new Pose2d(0.9172529578208923, 2.905292844772339, Rotation2d.kCW_90deg); + public static final Pose2d leftClimbPose = + new Pose2d(1.114691138267517, 4.505441703796387, Rotation2d.kCCW_90deg); + + public static final int climberMotorID = 25; + public static final double climbSpeed = 0.80; + + public static final Distance maxHeight = Inches.of(9); + public static final Distance minHeight = Inches.of(0); + + public static final double drumRadiusInches = 0.5; + + public static final double climberGearBox = (25 / 1); + + public static final double climberSensorToMechanismRatio = + climberGearBox / (2 * Math.PI * Units.inchesToMeters(drumRadiusInches)); + + public static final FeedbackConfigs feedbackConfigs = + new FeedbackConfigs().withSensorToMechanismRatio(climberSensorToMechanismRatio); + + public static final MotorOutputConfigs motorOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake); + + public static final SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitThreshold(maxHeight.in(Meters)) + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitThreshold(minHeight.in(Meters)) + .withReverseSoftLimitEnable(true); + public static final CurrentLimitsConfigs currentLimitConfigs = + new CurrentLimitsConfigs().withStatorCurrentLimit(45).withStatorCurrentLimitEnable(true); + + public static final TalonFXConfiguration climberConfigs = + new TalonFXConfiguration() + .withCurrentLimits(currentLimitConfigs) + .withFeedback(feedbackConfigs) + .withMotorOutput(motorOutputConfigs) + .withSoftwareLimitSwitch(softwareLimitSwitchConfigs); + } + public static class IntakeConstants { public static final int armMainID = 14; public static final int armFollowerID = 15; public static final int intakeID = 16; public static final int armMagnetID = 17; - public static final double armGearRatio = (5 / 1) * (36 / 10); + public static final double armGearRatio = 36 / 12; // 111.182298 - 15.025 = 96; public static final Angle minPosition = Degrees.of(0.0); public static final Angle maxPosition = Degrees.of(96.157298); @@ -217,8 +325,8 @@ public static class IntakeConstants { public static final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(RotationsPerSecond.of(0)) - .withMotionMagicAcceleration(RotationsPerSecondPerSecond.of(0)); + .withMotionMagicCruiseVelocity(RotationsPerSecond.of(40)) + .withMotionMagicAcceleration(RotationsPerSecondPerSecond.of(80)); public static final Slot0Configs slot0Configs = new Slot0Configs() @@ -226,10 +334,11 @@ public static class IntakeConstants { .withKV(0.0) .withKA(0.0) .withKG(0.00) - .withKP(0.0) + .withKP(20.0) .withKI(0.00) .withKD(0.00) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign); + .withGravityType(GravityTypeValue.Arm_Cosine) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); public static final FeedbackConfigs feedbackConfigs = new FeedbackConfigs().withSensorToMechanismRatio(armGearRatio); @@ -392,14 +501,15 @@ public static class FieldConstants { new Pose2d(2.0, 3.118569850921631, Rotation2d.kZero), new Pose2d(2.0, 1.4229397773742676, Rotation2d.kZero)); + public static final Distance TRENCH_LENGTH = Inches.of(47); public static final Distance TRENCH_BUMP_X = Inches.of(181.56); private static final Distance TRENCH_WIDTH = Inches.of(49.86); private static final Distance BUMP_INSET = TRENCH_WIDTH.plus(Inches.of(12)); private static final Distance BUMP_LENGTH = Inches.of(73); public static final Distance BUMP_CENTER_Y = TRENCH_WIDTH.plus(BUMP_LENGTH.div(2)); - private static final Distance TRENCH_ZONE_EXTENSION = Inches.of(70); - private static final Distance BUMP_ZONE_EXTENSION = Inches.of(60); + public static final Distance TRENCH_ZONE_EXTENSION = Inches.of(70); + public static final Distance BUMP_ZONE_EXTENSION = Inches.of(60); private static final Distance TRENCH_BUMP_ZONE_TRANSITION = TRENCH_WIDTH.plus(BUMP_INSET).div(2); @@ -549,10 +659,12 @@ public static class AutoConstants { public static class HoodConstants { public static final int hoodMotorID = 21; // 21 - public static final double hoodStallCurrent = 8; // amps + public static final double hoodStallCurrent = 10; // amps public static final double hoodStallVelocity = 0.1353; // rps public static final double hoodZeroSpeed = -0.15; + public static final double slowHoodSpeed = 0.20; + public static final Angle minAngle = Degrees.of(21.448); public static final Angle maxAngle = Degrees.of(59.231); @@ -611,6 +723,12 @@ public static class SpindexerConstants { public static final double spindexerMotorSpeed = 0.5; public static final double kickerMotorSpeed = 0.9; + public static final double spindexerIdleSpeed = -0.15; + public static final double kickerIdleSpeed = -0.15; + + public static final double reverseKickerSpeed = -0.30; + public static final double reverseSpindexerSpeed = -0.30; + public static final MotorOutputConfigs motorOutputConfigs = new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f37eed7..40d5b7e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -204,10 +204,10 @@ public void simulationPeriodic() { FuelSim.getInstance().updateSim(); FuelSim.getInstance() .toggleAirResistance(SmartDashboard.getBoolean("Air Resistance Toggle", false)); - FuelSim.Hub.RED_HUB.toggleScoreWhenActive( - SmartDashboard.getBoolean("Only Score while Active", false)); - FuelSim.Hub.BLUE_HUB.toggleScoreWhenActive( - SmartDashboard.getBoolean("Only Score while Active", false)); + FuelSim.Hub.RED_HUB.toggleCountWhenActive( + SmartDashboard.getBoolean("Only Count while Active", false)); + FuelSim.Hub.BLUE_HUB.toggleCountWhenActive( + SmartDashboard.getBoolean("Only Count while Active", false)); SmartDashboard.putNumber("Red Alliance Score", FuelSim.Hub.RED_HUB.getScore()); SmartDashboard.putNumber("Blue Alliance Score", FuelSim.Hub.BLUE_HUB.getScore()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 69036a1..c877d1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -26,10 +27,11 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.Constants.SOTMConstants; import frc.robot.Constants.SwerveConstants; +import frc.robot.commands.GuidedTeleopSwerve; import frc.robot.commands.MoveToFuel; import frc.robot.commands.ShootOnTheMove; -import frc.robot.commands.TeleopSwerve; import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Hood; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -91,9 +93,12 @@ public class RobotContainer { @Logged(name = "Spindexer") private final Spindexer spindexer = new Spindexer(); + @Logged(name = "Climber") + private final Climber climber = new Climber(); + @Logged(name = "3D Visualization") private final RobotVisualization robotVisualization = - new RobotVisualization(turret, hood, swerve, shooter); + new RobotVisualization(turret, hood, swerve, shooter, climber); @Logged(name = "Fuel Sim") private final FuelSim fuelInstance = FuelSim.getInstance(); @@ -121,7 +126,7 @@ public class RobotContainer { .getTranslation() .getDistance(AllianceUtil.getHubPose().getTranslation()); - double timeOfFlight = SOTMConstants.timeOfFlightMap.get(distanceToHub); + double timeOfFlight = SOTMConstants.timeOfFlightMapScoring.get(distanceToHub); return (timeUntilActive) <= timeOfFlight; }) @@ -142,7 +147,8 @@ public RobotContainer() { shooter, spindexer, AllianceUtil::getHubPose, - robotVisualization)); + robotVisualization, + ()-> true)); NamedCommands.registerCommand( "Pathfind to Mid-Left Bumper", swerve.pathFindToPose(FieldConstants.midLBumperPose)); @@ -152,7 +158,7 @@ public RobotContainer() { NamedCommands.registerCommand( "FerryOTM", new ShootOnTheMove( - swerve, turret, hood, shooter, spindexer, ferryPoseSupplier, robotVisualization)); + swerve, turret, hood, shooter, spindexer, ferryPoseSupplier, robotVisualization, ()-> false)); NamedCommands.registerCommand( "IntakeUntilFull", Commands.waitUntil(() -> !robotVisualization.canSimIntake())); @@ -169,7 +175,8 @@ public RobotContainer() { shooter, spindexer, AllianceUtil::getHubPose, - robotVisualization) + robotVisualization, + ()-> true) .until(() -> robotVisualization.isEmpty())); NamedCommands.registerCommand( @@ -191,30 +198,30 @@ public RobotContainer() { configureFuelSim(); } - SmartDashboard.putData(intake.zeroArmCommand()); - SmartDashboard.putData("Zero Hood", hood.zeroHoodCommand()); + // SmartDashboard.putData(intake.zeroArmCommand()); + // SmartDashboard.putData("Zero Hood", hood.zeroHoodCommand()); SmartDashboard.putNumber("Dynamic Shooter Speed", 0); SmartDashboard.putNumber("Dynamic Hood Angle", HoodConstants.minAngle.in(Degrees)); - // goalShotTarget = AllianceUtil.getHubPose(); + goalShotTarget = AllianceUtil.getHubPose(); - // inAllianceZoneTrigger.onTrue( - // Commands.runOnce(() -> goalShotTarget = AllianceUtil.getHubPose())); + inAllianceZoneTrigger.onTrue( + Commands.runOnce(() -> goalShotTarget = AllianceUtil.getHubPose())); - // onLeftSideTrigger - // .and(inAllianceZoneTrigger.negate()) - // .onTrue(Commands.runOnce(() -> goalShotTarget = leftFerryPose.get())); + onLeftSideTrigger + .and(inAllianceZoneTrigger.negate()) + .onTrue(Commands.runOnce(() -> goalShotTarget = leftFerryPose.get())); - // onLeftSideTrigger - // .negate() - // .and(inAllianceZoneTrigger.negate()) - // .onTrue(Commands.runOnce(() -> goalShotTarget = rightFerryPose.get())); + onLeftSideTrigger + .negate() + .and(inAllianceZoneTrigger.negate()) + .onTrue(Commands.runOnce(() -> goalShotTarget = rightFerryPose.get())); - // preShiftShoot.onTrue(driverController.rumbleFor(RumbleType.kBothRumble, 1.0, 1)); + preShiftShoot.onTrue(driverController.rumbleFor(RumbleType.kBothRumble, 1.0, 1)); - // preShiftShoot.onTrue(Commands.runOnce(() -> canPreShoot = true)); - // activeHubTrigger.onFalse(Commands.runOnce(() -> canPreShoot = false)); + preShiftShoot.onTrue(Commands.runOnce(() -> canPreShoot = true)); + activeHubTrigger.onFalse(Commands.runOnce(() -> canPreShoot = false)); } private void configureFuelSim() { @@ -248,15 +255,89 @@ private void configureFuelSim() { .ignoringDisable(true)); SmartDashboard.putBoolean("Air Resistance Toggle", false); - SmartDashboard.putBoolean("Only Score while Active", false); + SmartDashboard.putBoolean("Only Count while Active", false); SmartDashboard.putBoolean("Automated Shooting Toggle", false); } private void configureDriverBindings() { Trigger slowMode = driverController.leftTrigger(); - Trigger manualOverrideButton = driverController.rightStick(); - Trigger shootButton = driverController.rightTrigger(); + Trigger driveOverrideButton = driverController.rightTrigger(); Trigger resetHeadingButton = driverController.start().and(driverController.back()); + Trigger leftAutoClimbButton = driverController.leftBumper(); + Trigger rightAutoClimbButton = driverController.rightBumper(); + + resetHeadingButton.onTrue(swerve.runOnce(swerve::seedFieldCentric).ignoringDisable(true)); + + swerve.setDefaultCommand( + new GuidedTeleopSwerve( + driverController::getLeftY, + driverController::getLeftX, + driverController::getRightX, + () -> { + if (slowMode.getAsBoolean()) { + return SwerveConstants.slowModeMaxTranslationalSpeed; + } + return SwerveConstants.maxTranslationalSpeed; + }, + () -> driveOverrideButton.getAsBoolean(), + swerve)); + + // driverController + // .leftBumper() + // .whileTrue(intake.run(() -> intake.moveUpManual())) + // .onFalse(intake.stopArm()); + // driverController + // .rightBumper() + // .whileTrue(intake.run(() -> intake.moveDownManual())) + // .onFalse(intake.stopArm()); + + // leftAutoClimbButton.whileTrue( + // new AutoClimb( + // driverController::getLeftY, + // driverController::getLeftX, + // () -> { + // if (slowMode.getAsBoolean()) { + // return SwerveConstants.slowModeMaxTranslationalSpeed; + // } + // return SwerveConstants.maxTranslationalSpeed; + // }, + // swerve, + // climber, + // true)); + // rightAutoClimbButton.whileTrue( + // new AutoClimb( + // driverController::getLeftY, + // driverController::getLeftX, + // () -> { + // if (slowMode.getAsBoolean()) { + // return SwerveConstants.slowModeMaxTranslationalSpeed; + // } + // return SwerveConstants.maxTranslationalSpeed; + // }, + // swerve, + // climber, + // false)); + } + + private void configureOperatorBindings() { + Trigger shootButton = operatorController.rightTrigger(); + Trigger intakeSequenceButton = operatorController.leftTrigger(); + Trigger intakeRollerButton = operatorController.leftBumper(); + Trigger shakeIntakeButton = operatorController.rightBumper(); + Trigger zeroButton = operatorController.back().and(operatorController.start()); + Trigger reverseSpindexerButton = + operatorController.start().and(operatorController.back().negate()); + Trigger reverseIntakeButton = + operatorController.back().and(operatorController.start().negate()); + Trigger climberUpButton = operatorController.a(); + Trigger climberDownButton = operatorController.x(); + Trigger manualTurretForward = operatorController.povUp(); + Trigger manualTurretLeft = operatorController.povLeft(); + Trigger manualTurretRight = operatorController.povRight(); + Trigger manualTurretBackward = operatorController.povDown(); + Trigger manualHoodUpButton = operatorController.y(); + Trigger manualHoodDownButton = operatorController.b(); + Trigger manualShootButton = operatorController.leftStick(); // (activeHubTrigger.or(() -> canPreShoot)) // .and(automatedShootingTrigger) @@ -273,152 +354,56 @@ private void configureDriverBindings() { // goalShotTargetSupplier, // robotVisualization)); - // swerve.setDefaultCommand( - // new GuidedTeleopSwerve( - // driverController::getLeftY, - // driverController::getLeftX, - // driverController::getRightX, - // () -> { - // if (slowMode.getAsBoolean()) { - // return SwerveConstants.slowModeMaxTranslationalSpeed; - // } - // return SwerveConstants.maxTranslationalSpeed; - // }, - // () -> manualOverrideButton.getAsBoolean() || shootButton.getAsBoolean(), - // swerve)); - - swerve.setDefaultCommand( - new TeleopSwerve( - driverController::getLeftY, - driverController::getLeftX, - driverController::getRightX, - () -> { - if (slowMode.getAsBoolean()) { - return SwerveConstants.slowModeMaxTranslationalSpeed; - } - return SwerveConstants.maxTranslationalSpeed; - }, - swerve)); - - driverController - .rightBumper() - .whileTrue(intake.run(() -> intake.setIntakeSpeed(0.5))) - .onFalse(intake.stop()); - driverController - .leftBumper() - .whileTrue(spindexer.run(() -> spindexer.runBoth())) - .onFalse(spindexer.runOnce(() -> spindexer.stopBoth())); - // driverController - // .rightTrigger() - // .whileTrue(spindexer.run(() -> spindexer.runBoth())) - // .onFalse(spindexer.runOnce(() -> spindexer.stopBoth())); - driverController - .rightTrigger() - .whileTrue( - new ShootOnTheMove( - swerve, - turret, - hood, - shooter, - spindexer, - AllianceUtil::getHubPose, - robotVisualization)); + turret.setDefaultCommand(turret.faceTarget(AllianceUtil::getHubPose, swerve::getRobotPose)); + spindexer.setDefaultCommand(spindexer.idleReverse()); + // intake.setDefaultCommand(intake.intakeSequence(false)); + // hood.setDefaultCommand(hood.aimForTarget(AllianceUtil::getHubPose, swerve::getRobotPose)); + hood.setDefaultCommand(hood.moveToAngleCommand(HoodConstants.minAngle)); - // driverController.b().whileTrue(turret.run(() -> - // turret.runTurret(0.15))).onFalse(turret.stop()); - // driverController - // .a() - // .whileTrue(turret.run(() -> turret.runTurret(-0.15))) - // .onFalse(turret.stop()); - // driverController.y().onTrue(hood.runOnce(() -> hood.zeroHood())); - driverController.y().onTrue(hood.zeroHoodCommand()); - driverController - .x() - .whileTrue(turret.faceTarget(AllianceUtil::getHubPose, swerve::getRobotPose)) - .onFalse(turret.moveToAngleCommand(Degrees.of(0))); - // driverController - // .povLeft() - // .whileTrue(hood.moveToAngleCommand(Degrees.of(50))) - // .onFalse(hood.moveToAngleCommand(Degrees.of(25))); - // driverController - // .povRight() - // .whileTrue(hood.moveToAngleCommand(Degrees.of(35))) - // .onFalse(hood.moveToAngleCommand(Degrees.of(25))); - // driverController - // .povUp() - // .whileTrue(turret.moveToAngleCommand(Degrees.of(180))) - // .onFalse(turret.moveToAngleCommand(Degrees.of(0))); - // driverController - // .povDown() - // .whileTrue(turret.moveToAngleCommand(Degrees.of(90))) - // .onFalse(turret.moveToAngleCommand(Degrees.of(0))); + shooter.setDefaultCommand(shooter.stopShooterCommand()); - resetHeadingButton.onTrue(swerve.runOnce(swerve::seedFieldCentric).ignoringDisable(true)); + shootButton.whileTrue( + new ShootOnTheMove( + swerve, + turret, + hood, + shooter, + spindexer, + goalShotTargetSupplier, + robotVisualization, + inAllianceZoneTrigger)); - // turret.setDefaultCommand(turret.faceTarget(AllianceUtil::getHubPose, swerve::getRobotPose)); - // turret.setDefaultCommand(turret.moveToAngleCommand(Degrees.of(0))); + manualTurretForward.whileTrue(turret.moveToAngleCommand(Degrees.of(0))); + manualTurretRight.whileTrue(turret.moveToAngleCommand(Degrees.of(90))); + manualTurretBackward.whileTrue(turret.moveToAngleCommand(Degrees.of(180))); + manualTurretLeft.whileTrue(turret.moveToAngleCommand(Degrees.of(-90))); - // hood.setDefaultCommand(hood.moveToAngleCommand(HoodConstants.minAngle)); + intakeSequenceButton.onTrue( + Commands.either( + intake.intakeSequence(false), intake.intakeSequence(true), intake::isIntakeDeployed)); - // shooter.setDefaultCommand(shooter.stopShooter()); + intakeRollerButton.onTrue(intake.toggleRollers()); - // intake.setDefaultCommand(intake.intakeToPosition(false)); + shakeIntakeButton.onTrue(intake.shakeIntakeCommand()).onFalse(intake.intakeSequence(true)); - // shootButton.whileTrue( - // new ShootOnTheMove( - // swerve, turret, hood, shooter, spindexer, goalShotTargetSupplier, - // robotVisualization)); - } + reverseSpindexerButton.whileTrue(spindexer.reverseBoth()); - private void configureOperatorBindings() { - (activeHubTrigger.or(() -> canPreShoot)) - .and(automatedShootingTrigger) - .and(inAllianceZoneTrigger) - // .and(shootButton.negate()) - .whileTrue( - new ShootOnTheMove( - swerve, - turret, - hood, - shooter, - spindexer, - goalShotTargetSupplier, - robotVisualization)); + reverseIntakeButton.whileTrue(intake.reverseIntakeRollers()).onFalse(intake.stopIntake()); - Trigger ferryMode = operatorController.leftTrigger(); - // turret.setDefaultCommand(turret.faceTarget(AllianceUtil::getHubPose, swerve::getRobotPose)); + climberUpButton.whileTrue(climber.climberUpCommand()); + climberDownButton.whileTrue(climber.climberDownCommand()); - // hood.setDefaultCommand(hood.aimForTarget(AllianceUtil::getHubPose, swerve::getRobotPose)); + manualHoodUpButton.whileTrue(hood.moveHoodCommand(true)).onFalse(hood.holdPosition()); + manualHoodDownButton.whileTrue(hood.moveHoodCommand(false)).onFalse(hood.holdPosition()); - ferryMode.whileTrue( - new ShootOnTheMove( - swerve, turret, hood, shooter, spindexer, ferryPoseSupplier, robotVisualization)); + manualShootButton.whileTrue( + shooter.manualInterpolatedShoot(() -> swerve.getTurretToHubMeters())); - operatorController - .y() - .onTrue( - Commands.runOnce( - () -> { - ferryPoseIndex = (ferryPoseIndex + 1) % FieldConstants.blueFerryPoints.size(); - swerve.updateFerryPoseDashboard(ferryPoseIndex); - })); - // operatorController.a().onTrue( - // Commands.runOnce(() -> { - // ferryPoseIndex = 0; - // updateFerryPoseDashboard(); - // })); - - // operatorController.b().onTrue( - // Commands.runOnce(() -> { - // ferryPoseIndex = 1; - // updateFerryPoseDashboard(); - // })); - - // operatorController.x().onTrue( - // Commands.runOnce(() -> { - // ferryPoseIndex = 2; - // updateFerryPoseDashboard(); - // })); + zeroButton.onTrue( + turret + .resetTurretPosition() + .alongWith(intake.zeroArmCommand()) + .alongWith(hood.zeroHoodCommand())); } private void configureAutoChooser() { diff --git a/src/main/java/frc/robot/commands/AutoClimb.java b/src/main/java/frc/robot/commands/AutoClimb.java new file mode 100644 index 0000000..8481c23 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoClimb.java @@ -0,0 +1,46 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ClimbConstants; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Swerve; +import frc.robot.util.AllianceUtil; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +/* 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 AutoClimb extends SequentialCommandGroup { + /** Creates a new AutoClimb. */ + public AutoClimb( + DoubleSupplier forwardSupplier, + DoubleSupplier strafeSupplier, + Supplier maxTranslationalSpeed, + Swerve swerve, + Climber climber, + boolean leftAlign) { + Supplier targetClimbPosition = + () -> { + List blueClimbingPoses = + List.of(ClimbConstants.leftClimbPose, ClimbConstants.rightClimbPose); + + List climbingPoses = + blueClimbingPoses.stream().map(AllianceUtil::flipPose).toList(); + + return leftAlign ? climbingPoses.get(0) : climbingPoses.get(1); + }; + + addCommands( + new RotateToPose( + forwardSupplier, strafeSupplier, targetClimbPosition, maxTranslationalSpeed, swerve) + .withDeadline(climber.climberUpCommand()), + new DriveToPose(swerve, false, targetClimbPosition), + climber.climberDownCommand()); + } +} diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java new file mode 100644 index 0000000..81f3d3d --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -0,0 +1,199 @@ +// 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 frc.robot.commands; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; + +import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.SwerveConstants; +import frc.robot.subsystems.Swerve; +import java.util.function.Supplier; + +public class DriveToPose extends Command { + private SwerveRequest.FieldCentric fieldOriented = + new SwerveRequest.FieldCentric() + .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective) + .withSteerRequestType(SteerRequestType.Position); + + private static final double drivekP = 2.0; + private static final double drivekD = 0.15; + private static final double thetakP = 8.0; + private static final double thetakD = 0.10; + + private static final double driveMaxVelocity = + SwerveConstants.maxTranslationalSpeed.in(MetersPerSecond); + private static final double driveMaxAcceleration = + SwerveConstants.maxTransationalAcceleration.in(MetersPerSecondPerSecond); + private static final double thetaMaxVelocity = + Units.rotationsToRadians(SwerveConstants.maxRotationalSpeed.in(RotationsPerSecond)); + private static final double thetaMaxAcceleration = + Units.rotationsToRadians( + SwerveConstants.maxAngularAcceleration.in(RotationsPerSecondPerSecond)); + + private static final double driveTolerance = 0.02; // 0.02 + private static final double thetaTolerance = Units.degreesToRadians(1.0); + + private static final double approachVelocityScale = 0.1; + private static final double approachDistanceThreshold = 1.0; + + private final Swerve swerve; + + private final Supplier target; + + private final ProfiledPIDController driveController = + new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0), 0.02); + private final ProfiledPIDController thetaController = + new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0), 0.02); + + private Translation2d lastSetpointTranslation = new Translation2d(); + private double driveErrorAbs = 0.0; + private double thetaErrorAbs = 0.0; + private boolean running = false; + private boolean rotateOnly; + + public DriveToPose(Swerve swerve, boolean rotateOnly, Supplier target) { + this.swerve = swerve; + this.target = target; + this.rotateOnly = rotateOnly; + + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + addRequirements(swerve); + } + + @Override + public void initialize() { + SmartDashboard.putNumber("DriveToPose/target/XPosition", target.get().getX()); + SmartDashboard.putNumber("DriveToPose/target/YPosition", target.get().getY()); + SmartDashboard.putNumber( + "DriveToPose/target/ThetaPosition", target.get().getRotation().getDegrees()); + + Pose2d currentPose = swerve.getRobotPose(); + Translation2d linearFieldVelocity = + new Translation2d( + swerve.getState().Speeds.vxMetersPerSecond, swerve.getState().Speeds.vyMetersPerSecond); + driveController.reset( + currentPose.getTranslation().getDistance(target.get().getTranslation()), + Math.min( + 0.0, + -linearFieldVelocity + .rotateBy( + target + .get() + .getTranslation() + .minus(currentPose.getTranslation()) + .getAngle() + .unaryMinus()) + .getX())); + thetaController.reset( + currentPose.getRotation().getRadians(), swerve.getState().Speeds.omegaRadiansPerSecond); + lastSetpointTranslation = currentPose.getTranslation(); + } + + @Override + public void execute() { + running = true; + + driveController.setP(drivekP); + driveController.setD(drivekD); + driveController.setConstraints( + new TrapezoidProfile.Constraints(driveMaxVelocity, driveMaxAcceleration)); + driveController.setTolerance(driveTolerance); + + thetaController.setP(thetakP); + thetaController.setD(thetakD); + thetaController.setConstraints( + new TrapezoidProfile.Constraints(thetaMaxVelocity, thetaMaxAcceleration)); + thetaController.setTolerance(thetaTolerance); + + Pose2d currentPose = swerve.getRobotPose(); + Pose2d targetPose = target.get(); + + double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation()); + driveErrorAbs = currentDistance; + + driveController.reset( + lastSetpointTranslation.getDistance(targetPose.getTranslation()), + driveController.getSetpoint().velocity); + + double driveVelocityScalar = driveController.calculate(driveErrorAbs, 0.0); + + boolean inApproachZone = currentDistance < approachDistanceThreshold; + if (inApproachZone) { + // double scale = MathUtil.clamp(currentDistance / approachDistanceThreshold, 0.1, 1.0); + // driveVelocityScalar *= scale * approachVelocityScale; + driveVelocityScalar *= Math.max(currentDistance / approachDistanceThreshold, 0.5); + } + + if (currentDistance < driveController.getPositionTolerance()) { + driveVelocityScalar = 0.0; + } + + lastSetpointTranslation = + new Pose2d( + targetPose.getTranslation(), + currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle()) + .transformBy( + new Transform2d( + new Translation2d(driveController.getSetpoint().position, 0.0), + new Rotation2d())) + .getTranslation(); + + double thetaVelocity = + thetaController.calculate( + currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + thetaErrorAbs = + Math.abs(currentPose.getRotation().minus(targetPose.getRotation()).getRadians()); + + if (thetaErrorAbs < thetaController.getPositionTolerance()) { + thetaVelocity = 0.0; + } + + Translation2d driveVelocity = + new Pose2d( + new Translation2d(), + targetPose.getTranslation().minus(currentPose.getTranslation()).getAngle()) + .transformBy( + new Transform2d(new Translation2d(driveVelocityScalar, 0.0), new Rotation2d())) + .getTranslation(); + if (rotateOnly) { + swerve.setControl( + fieldOriented.withVelocityX(0).withVelocityY(0).withRotationalRate(thetaVelocity)); + } else { + swerve.setControl( + fieldOriented + .withVelocityX(driveVelocity.getX()) + .withVelocityY(driveVelocity.getY()) + .withRotationalRate(thetaVelocity)); + } + SmartDashboard.putBoolean("DriveToPose/Theta At Goal", thetaController.atGoal()); + SmartDashboard.putBoolean("DriveToPose/Translation At Goal", driveController.atGoal()); + } + + @Override + public void end(boolean interrupted) { + swerve.setControl(fieldOriented.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + running = false; + } + + @Override + public boolean isFinished() { + return running && driveController.atGoal() && thetaController.atGoal(); + } +} diff --git a/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java b/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java index 7fbd7f0..cfadb32 100644 --- a/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java +++ b/src/main/java/frc/robot/commands/GuidedTeleopSwerve.java @@ -63,8 +63,13 @@ public class GuidedTeleopSwerve extends Command { private final Trigger inBumpZoneTrigger; - private final PIDController trenchYController = new PIDController(3.53, 0, 0); - private final PIDController rotationController = new PIDController(8, 0, 0); + private final PIDController trenchYController = new PIDController(4.0, 0, 0.05); + private final PIDController rotationController = new PIDController(8, 0, 0.10); + + private final double trenchYControllerTolerance = 0.02; + private final double trenchYVelocityTolerance = 0.0353; + private final double rotationControllerTolerance = Units.degreesToRadians(1); + private final double rotationVelocityTolerance = Units.degreesToRadians(0.5); private DriveMode currentDriveMode = DriveMode.NormalDrive; @@ -99,6 +104,7 @@ private Command updateDriveMode(DriveMode driveMode) { private double getDotProduct() { Pose2d robotPose = swerve.getRobotPose(); + Pose2d targetPose = inTrenchZoneTrigger.getAsBoolean() ? swerve.getClosestTrenchPose() @@ -124,7 +130,7 @@ private double getMaxTranslationalSpeed() { private double getForwardSpeed() { double forwardSpeed = forwardRateLimiter.calculate(-forwardSupplier.getAsDouble() * getMaxTranslationalSpeed()); - if (Math.abs(forwardSupplier.getAsDouble()) <= .065) { + if (Math.abs(forwardSupplier.getAsDouble()) <= 0.005) { forwardSpeed = 0; forwardRateLimiter.reset(0); } @@ -134,7 +140,7 @@ private double getForwardSpeed() { private double getStrafeSpeed() { double stafeSpeed = strafeRateLimiter.calculate(-strafeSupplier.getAsDouble() * getMaxTranslationalSpeed()); - if (Math.abs(strafeSupplier.getAsDouble()) <= .065) { + if (Math.abs(strafeSupplier.getAsDouble()) <= 0.005) { stafeSpeed = 0; strafeRateLimiter.reset(0); } @@ -150,11 +156,10 @@ private double getRotationSpeed() { double rotationSpeed = joystickRotation * SwerveConstants.maxRotationalSpeed.in(RotationsPerSecond); - if (Math.abs(rotationSpeed) <= (Units.degreesToRotations(1.5))) { + if (Math.abs(rotationSpeed) <= Units.degreesToRadians(1.5)) { rotationRateLimiter.reset(0); return 0; } - return rotationRateLimiter.calculate(rotationSpeed); } @@ -178,6 +183,14 @@ private void driveTrench(double forward) { yVel = MathUtil.clamp(yVel, -getMaxTranslationalSpeed(), getMaxTranslationalSpeed()); + if (trenchYController.atSetpoint() || Math.abs(yVel) < trenchYVelocityTolerance) { + yVel = 0; + } + + if (rotationController.atSetpoint() || Math.abs(rotStraightSpeed) < rotationVelocityTolerance) { + rotStraightSpeed = 0; + } + swerve.setControl( fieldOriented .withVelocityX(forward) @@ -192,6 +205,10 @@ private void driveBump(double forward, double strafe) { rotDiagonalSpeed = MathUtil.clamp(rotDiagonalSpeed, -maxRotSpeedRads, maxRotSpeedRads); + if (rotationController.atSetpoint() || Math.abs(rotDiagonalSpeed) < rotationVelocityTolerance) { + rotDiagonalSpeed = 0; + } + swerve.setControl( fieldOriented .withVelocityX(forward) @@ -202,6 +219,9 @@ private void driveBump(double forward, double strafe) { @Override public void initialize() { rotationController.enableContinuousInput(-Math.PI, Math.PI); + trenchYController.setTolerance(trenchYControllerTolerance); // 2 cm + rotationController.setTolerance(rotationControllerTolerance); // 2 cm + flipFactor = AllianceUtil.isRedAlliance() ? -1 : 1; } @@ -211,11 +231,16 @@ public void execute() { double strafeSpeed = getStrafeSpeed(); double rotSpeed = getRotationSpeed(); - double dotProduct = currentDriveMode == DriveMode.NormalDrive ? 0 : getDotProduct(); + double dotProduct = + ((currentDriveMode == DriveMode.NormalDrive) || swerve.inTrenchBox()) + ? 1.0 + : getDotProduct(); + // if midsts of trench then ignore the dot product and keep the robot centered + // also ignore dot product if we are in normal drive DriveMode effectiveDriveMode = - (manualOverrideSupplier.getAsBoolean() - || (dotProduct < 0.10)) // only lock if in trying to drive in that direction + (manualOverrideSupplier.getAsBoolean()) + || (dotProduct < 0.10) // only lock if in trying to drive in that direction ? DriveMode.NormalDrive : currentDriveMode; diff --git a/src/main/java/frc/robot/commands/RotateToPose.java b/src/main/java/frc/robot/commands/RotateToPose.java new file mode 100644 index 0000000..77c1083 --- /dev/null +++ b/src/main/java/frc/robot/commands/RotateToPose.java @@ -0,0 +1,145 @@ +// 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 frc.robot.commands; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; + +import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.SimConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.subsystems.Swerve; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class RotateToPose extends Command { + /** Creates a new TeleopSwerve. */ + private final Swerve swerve; + + private SwerveRequest.FieldCentric fieldOriented = + new SwerveRequest.FieldCentric() + .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective) + .withSteerRequestType(SteerRequestType.Position); + + private final DoubleSupplier forwardSupplier; + private final DoubleSupplier strafeSupplier; + + private final Supplier maxTranslationalSpeedSupplier; + + private final ProfiledPIDController thetaController = + new ProfiledPIDController( + 8.0, + 0.0, + 0.1, + new TrapezoidProfile.Constraints(thetaMaxVelocity, thetaMaxAcceleration), + SimConstants.loopPeriodSecs); + + private static final double thetaTolerance = Units.degreesToRadians(1.0); + private static final double thetaMaxVelocity = + Units.rotationsToRadians(SwerveConstants.maxRotationalSpeed.in(RotationsPerSecond)); + private static final double thetaMaxAcceleration = + Units.rotationsToRadians( + SwerveConstants.maxAngularAcceleration.in(RotationsPerSecondPerSecond)); + + private Supplier targetPoseSupplier; + + private SlewRateLimiter forwardRateLimiter = + new SlewRateLimiter(SwerveConstants.maxTransationalAcceleration.in(MetersPerSecondPerSecond)); + private SlewRateLimiter strafeRateLimiter = + new SlewRateLimiter(SwerveConstants.maxTransationalAcceleration.in(MetersPerSecondPerSecond)); + + public RotateToPose( + DoubleSupplier forwardSupplier, + DoubleSupplier strafeSupplier, + Supplier targetPoseSupplier, + Supplier maxTranslationalSpeed, + Swerve swerve) { + this.forwardSupplier = forwardSupplier; + this.strafeSupplier = strafeSupplier; + this.swerve = swerve; + this.maxTranslationalSpeedSupplier = maxTranslationalSpeed; + this.targetPoseSupplier = targetPoseSupplier; + + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + addRequirements(swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + thetaController.reset( + swerve.getState().Pose.getRotation().getRadians(), + swerve.getState().Speeds.omegaRadiansPerSecond); + + thetaController.setTolerance(thetaTolerance); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double maxTranslationalSpeed = maxTranslationalSpeedSupplier.get().in(MetersPerSecond); + double forwardSpeed = -forwardSupplier.getAsDouble() * maxTranslationalSpeed; + double strafeSpeed = -strafeSupplier.getAsDouble() * maxTranslationalSpeed; + + forwardSpeed = forwardRateLimiter.calculate(forwardSpeed); + strafeSpeed = strafeRateLimiter.calculate(strafeSpeed); + + if (Math.hypot(forwardSpeed, strafeSpeed) <= Units.inchesToMeters(0.5)) { + forwardSpeed = 0; + strafeSpeed = 0; + + forwardRateLimiter.reset(0); + strafeRateLimiter.reset(0); + } + + Pose2d currentPose = swerve.getState().Pose; + Pose2d targetPose = targetPoseSupplier.get(); + + double thetaVelocity = + thetaController.calculate( + currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + + double thetaErrorAbs = + Math.abs(currentPose.getRotation().minus(targetPose.getRotation()).getRadians()); + + if (thetaErrorAbs < thetaController.getPositionTolerance()) { + thetaVelocity = 0.0; + } + + swerve.setControl( + fieldOriented + .withVelocityX(forwardSpeed) + .withVelocityY(strafeSpeed) + .withRotationalRate(thetaVelocity)); + } + ; + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + forwardRateLimiter.reset(0); + strafeRateLimiter.reset(0); + swerve.setControl(fieldOriented.withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // return thetaController.atGoal(); + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ShootOnTheMove.java b/src/main/java/frc/robot/commands/ShootOnTheMove.java index 9758e9a..5686ac6 100644 --- a/src/main/java/frc/robot/commands/ShootOnTheMove.java +++ b/src/main/java/frc/robot/commands/ShootOnTheMove.java @@ -22,6 +22,7 @@ import frc.robot.util.RobotVisualization; import frc.robot.util.SOTMCalculator; import frc.robot.util.SOTMCalculator.ShootingParameters; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; /* 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 */ @@ -50,6 +51,7 @@ public class ShootOnTheMove extends Command { private double startTime; private boolean isVisualizationFirstShot = true; + private BooleanSupplier scoringMode; public ShootOnTheMove( Swerve swerve, @@ -58,7 +60,8 @@ public ShootOnTheMove( Shooter shooter, Spindexer spindexer, Supplier targetPoseSupplier, - RobotVisualization robotVisualization) { + RobotVisualization robotVisualization, + BooleanSupplier scoringMode) { this.swerve = swerve; this.turret = turret; this.hood = hood; @@ -66,6 +69,7 @@ public ShootOnTheMove( this.spindexer = spindexer; this.targetPoseSupplier = targetPoseSupplier; this.robotVisualization = robotVisualization; + this.scoringMode = scoringMode; addRequirements(hood, turret, shooter); } @@ -96,7 +100,13 @@ public void execute() { ShootingParameters shootingParameters = SOTMCalculator.getParameters( - swerve, turret, targetPoseSupplier.get(), fieldAccelX, fieldAccelY, fieldSpeeds); + swerve, + turret, + targetPoseSupplier.get(), + fieldAccelX, + fieldAccelY, + fieldSpeeds, + scoringMode.getAsBoolean()); turret.moveToAngle(shootingParameters.turretAngle()); hood.moveToAngle(shootingParameters.hoodAngle()); @@ -115,7 +125,7 @@ public void execute() { if (RobotBase.isSimulation()) { // if sim and ready to shoot if (isVisualizationFirstShot || ((Timer.getFPGATimestamp() - startTime) > 1 / SimConstants.fuelsPerSecond)) { - robotVisualization.shootFuel(); + robotVisualization.shootFuel(shootingParameters); startTime = Timer.getFPGATimestamp(); isVisualizationFirstShot = false; diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java new file mode 100644 index 0000000..baa25d5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,132 @@ +// 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 frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ClimbConstants; + +public class Climber extends SubsystemBase { + private TalonFX climberMotor; + + private StatusSignal climberPosition; + + /** Creates a new Spindexer. */ + private ElevatorSim climberSim; + + public Climber() { + climberMotor = new TalonFX(ClimbConstants.climberMotorID); + + climberMotor.getConfigurator().apply(ClimbConstants.climberConfigs); + + zeroClimber(); + + climberPosition = climberMotor.getPosition(); + + if (RobotBase.isSimulation()) { + climberSim = + new ElevatorSim( + DCMotor.getKrakenX60(1), + ClimbConstants.climberGearBox, + Units.lbsToKilograms(5), + Units.inchesToMeters(ClimbConstants.drumRadiusInches), + ClimbConstants.minHeight.in(Meters), + ClimbConstants.maxHeight.in(Meters), + true, + 0); + } + } + + public void zeroClimber() { + climberMotor.setPosition(0); + } + + public void stopClimber() { + climberMotor.stopMotor(); + } + + public Command stopClimberCommand() { + return runOnce(() -> stopClimber()); + } + + public Angle getClimberPosition() { + return climberPosition.getValue(); + } + + public Distance getClimberHeight() { + return Meters.of(getClimberPosition().in(Rotations)); + } + + public boolean atTop() { + return getClimberHeight().gte(ClimbConstants.maxHeight.minus(Inches.of(0.1))); + } + + public boolean atBottom() { + return getClimberHeight().lte(ClimbConstants.minHeight.plus(Inches.of(0.1))); + } + + public void setSpeed(double speed) { + climberMotor.set(speed); + } + + public Command moveForward() { + return run(() -> climberMotor.set(ClimbConstants.climbSpeed)); + } + + public Command moveReverse() { + return run(() -> climberMotor.set(-1 * ClimbConstants.climbSpeed)); + } + + public Command climberUpCommand() { + return run(() -> climberMotor.set(ClimbConstants.climbSpeed)) + .until(this::atTop) + .finallyDo(() -> stopClimber()); + } + + public Command climberDownCommand() { + return run(() -> climberMotor.set(-1 * ClimbConstants.climbSpeed)) + .until(this::atBottom) + .finallyDo(() -> stopClimber()); + } + + @Override + public void periodic() { + climberPosition.refresh(); + SmartDashboard.putNumber("Climber/ClimberPosition", getClimberPosition().in(Rotations)); + SmartDashboard.putNumber("Climber/ClimberHeight Inches", getClimberHeight().in(Inches)); + } + + @Override + public void simulationPeriodic() { + TalonFXSimState climberSimState = climberMotor.getSimState(); + + climberSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + + climberSim.setInputVoltage(climberSimState.getMotorVoltage()); + + climberSim.update(0.020); + + climberSimState.setRawRotorPosition( + climberSim.getPositionMeters() * ClimbConstants.climberSensorToMechanismRatio); + + climberSimState.setRotorVelocity( + climberSim.getVelocityMetersPerSecond() * ClimbConstants.climberSensorToMechanismRatio); + } +} diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index 41a97cd..178d23f 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.HoodConstants; import frc.robot.Constants.SOTMConstants; @@ -60,10 +61,10 @@ public Hood() { HoodConstants.hoodGearRatio, 0.00398184688, 0.1408176, - HoodConstants.minAngle.in(Radians), - HoodConstants.maxAngle.in(Radians), + 0, + HoodConstants.maxAngle.minus(HoodConstants.minAngle).in(Radians), true, - HoodConstants.minAngle.in(Radians)); + 0); } public void zeroHood() { @@ -71,23 +72,44 @@ public void zeroHood() { targetAngle = HoodConstants.minAngle; } + // public Command zeroHoodCommand() { + // return runOnce( + // () -> { + // hoodMotor.stopMotor(); + // hoodMotor.setPosition(HoodConstants.maxAngle); + // }) + // .andThen( + // run(() -> { + // hoodMotor.set(HoodConstants.hoodZeroSpeed); + // }) + // .withTimeout(1.0)) + // .andThen( + // () -> { + // hoodMotor.stopMotor(); + // hoodMotor.setPosition(HoodConstants.minAngle.minus(Degrees.of(0.1))); + // }) + // .withName("Zero Hood Command"); + // } + public Command zeroHoodCommand() { - return runOnce( + return Commands.startRun( + // run once () -> { hoodMotor.stopMotor(); hoodMotor.setPosition(HoodConstants.maxAngle); + }, + // run + () -> { + hoodMotor.set(HoodConstants.hoodZeroSpeed); }) - .andThen( - run(() -> { - hoodMotor.set(HoodConstants.hoodZeroSpeed); - }) - .withTimeout(1.0)) - .andThen( + .until( + () -> hoodMotor.getStatorCurrent().getValueAsDouble() > HoodConstants.hoodStallCurrent) + .finallyDo( () -> { hoodMotor.stopMotor(); - hoodMotor.setPosition(HoodConstants.minAngle.minus(Degrees.of(0.1))); - }) - .withName("Zero Hood Command"); + hoodMotor.setPosition(HoodConstants.minAngle); + hoodMotor.setControl(motionMagicRequest.withPosition(Degrees.of(25))); + }); } // public Command zeroHoodCommand() { @@ -122,12 +144,12 @@ public boolean atAngle(Angle target, Angle tolerance) { } public Angle getInterpolatedHoodAngle(double distanceMeters) { - return SOTMConstants.hoodAngleMap.get(distanceMeters).getMeasure(); + return SOTMConstants.hoodAngleMapScoring.get(distanceMeters).getMeasure(); } public Angle getInterpolatedHoodAngle(Pose2d poseA, Pose2d poseB) { double distance = poseA.getTranslation().getDistance(poseB.getTranslation()); - return SOTMConstants.hoodAngleMap.get(distance).getMeasure(); + return SOTMConstants.hoodAngleMapScoring.get(distance).getMeasure(); } public void stopHood() { @@ -140,7 +162,7 @@ public void moveHood(double speed) { public void moveToAngle(Angle targetHoodAngle) { targetAngle = targetHoodAngle; - hoodMotor.setControl(motionMagicRequest.withPosition(targetAngle)); + hoodMotor.setControl(motionMagicRequest.withPosition(targetHoodAngle)); } public Angle getTargetAngle() { @@ -151,6 +173,11 @@ public Command stop() { return runOnce(() -> hoodMotor.stopMotor()).withName("Stop Hood"); } + public Command moveHoodCommand(boolean upMovement) { + int flipFactor = upMovement ? 1 : -1; + return run(() -> moveHood(flipFactor * HoodConstants.slowHoodSpeed)); + } + public Command moveToAngleCommand(Angle targetPose) { return run(() -> { hoodMotor.setControl(motionMagicRequest.withPosition(targetPose)); @@ -158,6 +185,18 @@ public Command moveToAngleCommand(Angle targetPose) { .withName("Move Hood to Angle"); } + public Command holdPosition() { + return startRun( + () -> { + motionMagicRequest.Position = hoodPosition.getValueAsDouble(); + hoodMotor.setControl(motionMagicRequest); + }, + () -> { + hoodMotor.setControl(motionMagicRequest); + }) + .withName("Hood Hold"); + } + public Command aimForTarget( Supplier targetPoseSupplier, Supplier robotPoseSupplier) { return run(() -> { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index b16b010..7842ff9 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -4,9 +4,7 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.MotionMagicVoltage; @@ -15,27 +13,31 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { /*Objects */ private final TalonFX armMainMotor; - // private final TalonFX armFollowerMotor; + private final TalonFX armFollowerMotor; private final TalonFX intakeMotor; private final DigitalInput armMagnetSensor; private boolean wasDeployedLastLoop; + private final double shakePeriod = 3.53; + private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0).withEnableFOC(true); private StatusSignal armMainPosition; - // private StatusSignal armFollowerPosition; + private StatusSignal armFollowerPosition; private final StatusSignal armCurrent; private final StatusSignal armVelocity; @@ -44,18 +46,21 @@ public class Intake extends SubsystemBase { public Intake() { intakeMotor = new TalonFX(IntakeConstants.intakeID); armMainMotor = new TalonFX(IntakeConstants.armMainID); - // armFollowerMotor = new TalonFX(IntakeConstants.armFollowerID); + armFollowerMotor = new TalonFX(IntakeConstants.armFollowerID); armMagnetSensor = new DigitalInput(IntakeConstants.armMagnetID); armMainMotor.getConfigurator().apply(IntakeConstants.armConfigs); - // armFollowerMotor.getConfigurator().apply(IntakeConstants.armConfigs); + armFollowerMotor.getConfigurator().apply(IntakeConstants.armConfigs); armCurrent = armMainMotor.getStatorCurrent(); armVelocity = armMainMotor.getVelocity(); + armMainMotor.setPosition(0); + armFollowerMotor.setPosition(0); + armMainPosition = armMainMotor.getPosition(); - // armFollowerPosition = armFollowerMotor.getPosition(); + armFollowerPosition = armFollowerMotor.getPosition(); } public void setIntakeSpeed(double speed) { @@ -66,25 +71,65 @@ public Command startIntake() { return run(() -> intakeMotor.set(IntakeConstants.intakeSpeed)).withName("Start Intake"); } + public Command reverseIntakeRollers() { + return run(() -> intakeMotor.set(-1 * IntakeConstants.intakeSpeed)).withName("Reverse Intake"); + } + public Command stopIntake() { return runOnce(() -> intakeMotor.stopMotor()).withName("Stop Intake"); } - public Command stop() { - return runOnce(this::stopIntake).withName("Stop Intake"); + public Command stopArm() { + return runOnce( + () -> { + armMainMotor.stopMotor(); + armFollowerMotor.stopMotor(); + }) + .withName("Stop Intake"); + } + + public Command shakeIntakeCommand() { + return run( + () -> { + if (Timer.getFPGATimestamp() % shakePeriod < shakePeriod / 2) { + moveDown(); + } else { + moveUp(); + } + }); + } + + public void moveDown() { + armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.downPosition)); + armFollowerMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.downPosition)); + } + + public void moveDownManual() { + armMainMotor.set(0.5); + armFollowerMotor.set(0.5); + } + + public void moveUpManual() { + armMainMotor.set(0.5); + armFollowerMotor.set(0.5); + } + + public void moveUp() { + armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.upPosition)); + armFollowerMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.upPosition)); } public Command intakeToPosition(boolean downPosition) { return run(() -> { if (downPosition) { armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.downPosition)); - // armFollowerMotor.setControl( - // motionMagicRequest.withPosition(IntakeConstants.downPosition)); + armFollowerMotor.setControl( + motionMagicRequest.withPosition(IntakeConstants.downPosition)); } else { armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.upPosition)); - // armFollowerMotor.setControl( - // motionMagicRequest.withPosition(IntakeConstants.downPosition)); + armFollowerMotor.setControl( + motionMagicRequest.withPosition(IntakeConstants.upPosition)); } }) .withName("Intake To Position"); @@ -94,66 +139,89 @@ public Command intakeSequence(boolean intakeDown) { return run(() -> { if (intakeDown) { armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.downPosition)); - // armFollowerMotor.setControl( - // motionMagicRequest.withPosition(IntakeConstants.downPosition)); + armFollowerMotor.setControl( + motionMagicRequest.withPosition(IntakeConstants.downPosition)); setIntakeSpeed(IntakeConstants.intakeSpeed); } else { armMainMotor.setControl(motionMagicRequest.withPosition(IntakeConstants.upPosition)); - // armFollowerMotor.setControl( - // motionMagicRequest.withPosition(IntakeConstants.upPosition)); + armFollowerMotor.setControl( + motionMagicRequest.withPosition(IntakeConstants.upPosition)); stopIntake(); } }) .withName("Intake working"); } + public Command toggleRollers() { + return Commands.either( + // if rolelrs are already on stop them + stopIntake(), + + // if rollers off then move it down and then set them to the speed + run( + () -> { + moveDown(); + setIntakeSpeed(IntakeConstants.intakeSpeed); + }), + this::rollersRunning); + } + public boolean isIntakeDeployed() { // return !armMagnetSensor.get(); + // return armMainPosition.getValue().gte(IntakeConstants.armDownPositionTolerance); return true; // FOR TESTING IN SIM } - public Angle getArmAngle() { + public boolean rollersRunning() { + return Math.abs(intakeMotor.get()) > 0.05; + } + + public Angle getMainArmAngle() { return armMainPosition.getValue(); } public void setZero() { armMainMotor.setPosition(0); - // armFollowerMotor.setPosition(0); + armFollowerMotor.setPosition(0); } public Command zeroArmCommand() { - return Commands.run( - () -> { - armMainMotor.set(IntakeConstants.armZeroSpeed); - // armFollowerMotor.set(IntakeConstants.armZeroSpeed); - - armCurrent.refresh(); - armVelocity.refresh(); - }) - .until( - () -> - armCurrent.getValue().in(Amps) > IntakeConstants.armStallCurrent - && Math.abs(armVelocity.getValue().in(RotationsPerSecond)) - < IntakeConstants.armStallVelocity) - .andThen( - () -> { - armMainMotor.stopMotor(); - // armFollowerMotor.stopMotor(); - setZero(); - }) - .withName("Zero Arm Command"); + return new InstantCommand(); } + // public Command zeroArmCommand() { + // return Commands.run( + // () -> { + // armMainMotor.set(IntakeConstants.armZeroSpeed); + // // armFollowerMotor.set(IntakeConstants.armZeroSpeed); + + // armCurrent.refresh(); + // armVelocity.refresh(); + // }) + // .until( + // () -> + // armCurrent.getValue().in(Amps) > IntakeConstants.armStallCurrent + // && Math.abs(armVelocity.getValue().in(RotationsPerSecond)) + // < IntakeConstants.armStallVelocity) + // .andThen( + // () -> { + // armMainMotor.stopMotor(); + // // armFollowerMotor.stopMotor(); + // setZero(); + // }) + // .withName("Zero Arm Command"); + // } + @Override public void periodic() { - if (!isIntakeDeployed() && wasDeployedLastLoop) { - armMainMotor.setPosition(IntakeConstants.minPosition); - // armFollowerMotor.setPosition(IntakeConstants.minPosition); - } - wasDeployedLastLoop = isIntakeDeployed(); + // if (!isIntakeDeployed() && wasDeployedLastLoop) { + // armMainMotor.setPosition(IntakeConstants.minPosition); + // armFollowerMotor.setPosition(IntakeConstants.minPosition); + // } + // wasDeployedLastLoop = isIntakeDeployed(); armMainPosition.refresh(); - // armFollowerPosition.refresh(); + armFollowerPosition.refresh(); SmartDashboard.putNumber("Intake speed", intakeMotor.get()); SmartDashboard.putNumber("Intake Arm Position", armMainPosition.getValue().in(Rotations)); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index cf28b79..44ec1fd 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -10,10 +10,13 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.SOTMConstants; import frc.robot.Constants.ShooterConstants; +import java.util.function.DoubleSupplier; public class Shooter extends SubsystemBase { /** Creates a new Shooter. */ @@ -62,11 +65,11 @@ public AngularVelocity getCurrentVelocity() { } public boolean shooterAtSetPoint(LinearVelocity goalSpeed) { + if(RobotBase.isSimulation()) return true; LinearVelocity currentSpeed = angularToLinearVelocity(getCurrentVelocity()); return Math.abs(currentSpeed.in(MetersPerSecond) - goalSpeed.in(MetersPerSecond)) - < ShooterConstants.shooterSpeedTolerance.in(MetersPerSecond); - // return true; + < ShooterConstants.shooterSpeedTolerance.in(MetersPerSecond); } public Command runMotor(double speed) { @@ -77,18 +80,16 @@ public void stopMotor() { shooterMotor.set(0.0); } - // public void logSolution(ShotSolution solution) { - // currentShotSolution = solution; - // setGoalSpeed(solution.exitVelocity()); - // } - - // public ShotSolution getShotSolution() { - // return currentShotSolution; - // } - - // public LinearVelocity getExitVelocity() { - // return MetersPerSecond.of(9.353); - // } + public Command manualInterpolatedShoot(DoubleSupplier turretToHubMeters) { + return run( + () -> + shooterMotor.setControl( + velocityRequest.withVelocity( + linearToAngularVelocity( + MetersPerSecond.of( + SOTMConstants.shooterSpeedMapScoring.get( + turretToHubMeters.getAsDouble())))))); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Spindexer.java b/src/main/java/frc/robot/subsystems/Spindexer.java index c401101..6e18da3 100644 --- a/src/main/java/frc/robot/subsystems/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/Spindexer.java @@ -66,6 +66,22 @@ public void stopBoth() { kickerMotor.stopMotor(); } + public Command idleReverse() { + return run( + () -> { + spindexerMotor.set(SpindexerConstants.spindexerIdleSpeed); + kickerMotor.set(SpindexerConstants.kickerIdleSpeed); + }); + } + + public Command reverseBoth() { + return run( + () -> { + spindexerMotor.set(SpindexerConstants.reverseSpindexerSpeed); + kickerMotor.set(SpindexerConstants.reverseKickerSpeed); + }); + } + public Command runSpindexer() { return run(() -> spindexerMotor.set(SpindexerConstants.spindexerMotorSpeed)); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 775f42f..f453316 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -122,45 +122,42 @@ public static record TrackedFuel(Pose2d pose, double lastSeen) {} .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective) .withSteerRequestType(SteerRequestType.Position); - // private PhotonCamera arducamLeft = new PhotonCamera(VisionConstants.arducamLeftName); - // private PhotonCamera arducamRight = new PhotonCamera(VisionConstants.arducamRightName); + private PhotonCamera arducamLeft = new PhotonCamera(VisionConstants.arducamLeftName); + private PhotonCamera arducamRight = new PhotonCamera(VisionConstants.arducamRightName); private PhotonCamera arducamBackLeft = new PhotonCamera(VisionConstants.arducamBackLeftName); - // private PhotonCamera arducamBackRight = new PhotonCamera(VisionConstants.arducamBackRightName); + private PhotonCamera arducamBackRight = new PhotonCamera(VisionConstants.arducamBackRightName); // private PhotonCamera arducamFuel = new PhotonCamera(VisionConstants.arducamFuelName); - // private PhotonPoseEstimator leftPoseEstimator = - // new PhotonPoseEstimator(FieldConstants.aprilTagLayout, - // VisionConstants.arducamLeftTransform); + private PhotonPoseEstimator leftPoseEstimator = + new PhotonPoseEstimator(FieldConstants.aprilTagLayout, VisionConstants.arducamLeftTransform); - // private PhotonPoseEstimator rightPoseEstimator = - // new PhotonPoseEstimator(FieldConstants.aprilTagLayout, - // VisionConstants.arducamRightTransform); + private PhotonPoseEstimator rightPoseEstimator = + new PhotonPoseEstimator(FieldConstants.aprilTagLayout, VisionConstants.arducamRightTransform); private PhotonPoseEstimator backLeftPoseEstimator = new PhotonPoseEstimator( FieldConstants.aprilTagLayout, VisionConstants.arducamBackLeftTransform); - // private PhotonPoseEstimator backRightPoseEstimator = - // new PhotonPoseEstimator(FieldConstants.aprilTagLayout, - // VisionConstants.arducamRightTransform); + private PhotonPoseEstimator backRightPoseEstimator = + new PhotonPoseEstimator(FieldConstants.aprilTagLayout, VisionConstants.arducamBackRightTransform); - // private List latestArducamLeftResult; - // private List latestArducamRightResult; + private List latestArducamLeftResult; + private List latestArducamRightResult; private List latestArducamBackLeftResult; - // private List latestArducamBackRightResult; + private List latestArducamBackRightResult; // private List latestArducamFuelResult; - // private Optional> arducamLeftMatrix = Optional.empty(); - // private Optional> arducamLeftDistCoeffs = Optional.empty(); + private Optional> arducamLeftMatrix = Optional.empty(); + private Optional> arducamLeftDistCoeffs = Optional.empty(); - // private Optional> arducamRightMatrix = Optional.empty(); - // private Optional> arducamRightDistCoeffs = Optional.empty(); + private Optional> arducamRightMatrix = Optional.empty(); + private Optional> arducamRightDistCoeffs = Optional.empty(); private Optional> arducamBackLeftMatrix = Optional.empty(); private Optional> arducamBackLeftDistCoeffs = Optional.empty(); - // private Optional> arducamBackRightMatrix = Optional.empty(); - // private Optional> arducamBackRightDistCoeffs = Optional.empty(); + private Optional> arducamBackRightMatrix = Optional.empty(); + private Optional> arducamBackRightDistCoeffs = Optional.empty(); // private Optional> arducamFuelMatrix = Optional.empty(); // private Optional> arducamFuelDistCoeffs = Optional.empty(); @@ -171,11 +168,12 @@ public static record TrackedFuel(Pose2d pose, double lastSeen) {} private ExtendedVisionSystemSim visionSim; - // private PhotonCameraSim arducamLeftSim; - // private PhotonCameraSim arducamRightSim; + private PhotonCameraSim arducamLeftSim; + private PhotonCameraSim arducamRightSim; private PhotonCameraSim arducamBackLeftSim; - // private PhotonCameraSim arducamBackRightSim; + private PhotonCameraSim arducamBackRightSim; + // private PhotonCameraSim arducamFuelSim; @Logged(name = "Detected Targets") @@ -508,68 +506,65 @@ public Rotation2d getBumpLockAngle() { public Pose2d getClosestTrenchPose() { Pose2d robotPose = stateCache.Pose; - if (robotPose.getMeasureY().gte(FieldConstants.fieldWidth.div(2))) { - if (robotPose.getMeasureX().gte(FieldConstants.fieldLength.div(2))) { - return new Pose2d( - FieldConstants.fieldLength.minus(FieldConstants.TRENCH_BUMP_X).in(Meters), - FieldConstants.fieldWidth.minus(FieldConstants.TRENCH_CENTER).in(Meters), - Rotation2d.kZero); - } - return new Pose2d( - FieldConstants.TRENCH_BUMP_X.in(Meters), - FieldConstants.fieldWidth.minus(FieldConstants.TRENCH_CENTER).in(Meters), - Rotation2d.kZero); - } + boolean xHigh = robotPose.getX() >= FieldConstants.fieldLength.div(2).in(Meters); + boolean yHigh = robotPose.getY() >= FieldConstants.fieldWidth.div(2).in(Meters); - if (robotPose.getMeasureX().gte(FieldConstants.fieldLength.div(2))) { - return new Pose2d( - FieldConstants.fieldLength.minus(FieldConstants.TRENCH_BUMP_X).in(Meters), - FieldConstants.TRENCH_CENTER.in(Meters), - Rotation2d.kZero); - } + double x = + xHigh + ? FieldConstants.fieldLength.minus(FieldConstants.TRENCH_BUMP_X).in(Meters) + : FieldConstants.TRENCH_BUMP_X.in(Meters); + + double y = + yHigh + ? FieldConstants.fieldWidth.minus(FieldConstants.TRENCH_CENTER).in(Meters) + : FieldConstants.TRENCH_CENTER.in(Meters); + + return new Pose2d(x, y, Rotation2d.kZero); + } - return new Pose2d( - FieldConstants.TRENCH_BUMP_X.in(Meters), - FieldConstants.TRENCH_CENTER.in(Meters), - Rotation2d.kZero); + public boolean inTrenchBox() { + double robotX = stateCache.Pose.getX(); + + boolean xHigh = robotX >= FieldConstants.fieldLength.div(2).in(Meters); + + double x2 = + xHigh + ? FieldConstants.fieldLength + .minus(FieldConstants.TRENCH_BUMP_X) + .plus(FieldConstants.TRENCH_LENGTH.div(2)) + .in(Meters) + : FieldConstants.TRENCH_BUMP_X.plus(FieldConstants.TRENCH_LENGTH.div(2)).in(Meters); + double x1 = + xHigh + ? FieldConstants.fieldLength + .minus(FieldConstants.TRENCH_BUMP_X) + .minus(FieldConstants.TRENCH_LENGTH.div(2)) + .in(Meters) + : FieldConstants.TRENCH_BUMP_X.minus(FieldConstants.TRENCH_LENGTH.div(2)).in(Meters); + return robotX > x1 && robotX < x2; } public Pose2d getClosestBumpPose() { Pose2d robotPose = stateCache.Pose; - if (robotPose.getMeasureY().gte(FieldConstants.fieldWidth.div(2))) { - if (robotPose.getMeasureX().gte(FieldConstants.fieldLength.div(2))) { - return new Pose2d( - FieldConstants.fieldLength.minus(FieldConstants.TRENCH_BUMP_X).in(Meters), - FieldConstants.fieldWidth.minus(FieldConstants.BUMP_CENTER_Y).in(Meters), - Rotation2d.kZero); - } - return new Pose2d( - FieldConstants.TRENCH_BUMP_X.in(Meters), - FieldConstants.fieldWidth.minus(FieldConstants.BUMP_CENTER_Y).in(Meters), - Rotation2d.kZero); - } + boolean xHigh = robotPose.getX() >= FieldConstants.fieldLength.div(2).in(Meters); + boolean yHigh = robotPose.getY() >= FieldConstants.fieldWidth.div(2).in(Meters); - if (robotPose.getMeasureX().gte(FieldConstants.fieldLength.div(2))) { - return new Pose2d( - FieldConstants.fieldLength.minus(FieldConstants.TRENCH_BUMP_X).in(Meters), - FieldConstants.BUMP_CENTER_Y.in(Meters), - Rotation2d.kZero); - } + double x = + xHigh + ? FieldConstants.fieldLength.minus(FieldConstants.TRENCH_BUMP_X).in(Meters) + : FieldConstants.TRENCH_BUMP_X.in(Meters); - return new Pose2d( - FieldConstants.TRENCH_BUMP_X.in(Meters), - FieldConstants.BUMP_CENTER_Y.in(Meters), - Rotation2d.kZero); - } + double y = + yHigh + ? FieldConstants.fieldWidth.minus(FieldConstants.BUMP_CENTER_Y).in(Meters) + : FieldConstants.BUMP_CENTER_Y.in(Meters); - @Override - public void periodic() { - double startTime = Timer.getFPGATimestamp(); - stateCache = getState(); - SmartDashboard.putNumber("Swerve/Gyro Position", getPigeonRotation().getDegrees()); + return new Pose2d(x, y, Rotation2d.kZero); + } + public double getTurretToHubMeters() { Translation2d turretPose = stateCache .Pose @@ -579,36 +574,43 @@ public void periodic() { .getTranslation() .rotateBy(stateCache.Pose.getRotation())); - SmartDashboard.putNumber( - "Testing/Distance To Hub", - AllianceUtil.getHubPose().getTranslation().getDistance(turretPose)); + return AllianceUtil.getHubPose().getTranslation().getDistance(turretPose); + } + + @Override + public void periodic() { + double startTime = Timer.getFPGATimestamp(); + stateCache = getState(); + SmartDashboard.putNumber("Swerve/Gyro Position", getPigeonRotation().getDegrees()); + + SmartDashboard.putNumber("Testing/Distance To Hub", getTurretToHubMeters()); - // latestArducamLeftResult = arducamLeft.getAllUnreadResults(); - // latestArducamRightResult = arducamRight.getAllUnreadResults(); + latestArducamLeftResult = arducamLeft.getAllUnreadResults(); + latestArducamRightResult = arducamRight.getAllUnreadResults(); latestArducamBackLeftResult = arducamBackLeft.getAllUnreadResults(); - // latestArducamBackRightResult = arducamBackRight.getAllUnreadResults(); + latestArducamBackRightResult = arducamBackRight.getAllUnreadResults(); // latestArducamFuelResult = arducamFuel.getAllUnreadResults(); double stateTimestamp = Utils.currentTimeToFPGATime(stateCache.Timestamp); - // leftPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); - // rightPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); + leftPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); + rightPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); backLeftPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); - // backRightPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); + backRightPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); - // if (arducamLeftMatrix.isEmpty()) { - // arducamLeftMatrix = arducamLeft.getCameraMatrix(); - // } - // if (arducamLeftDistCoeffs.isEmpty()) { - // arducamLeftDistCoeffs = arducamLeft.getDistCoeffs(); - // } + if (arducamLeftMatrix.isEmpty()) { + arducamLeftMatrix = arducamLeft.getCameraMatrix(); + } + if (arducamLeftDistCoeffs.isEmpty()) { + arducamLeftDistCoeffs = arducamLeft.getDistCoeffs(); + } - // if (arducamRightMatrix.isEmpty()) { - // arducamRightMatrix = arducamRight.getCameraMatrix(); - // } - // if (arducamRightDistCoeffs.isEmpty()) { - // arducamRightDistCoeffs = arducamRight.getDistCoeffs(); - // } + if (arducamRightMatrix.isEmpty()) { + arducamRightMatrix = arducamRight.getCameraMatrix(); + } + if (arducamRightDistCoeffs.isEmpty()) { + arducamRightDistCoeffs = arducamRight.getDistCoeffs(); + } if (arducamBackLeftMatrix.isEmpty()) { arducamBackLeftMatrix = arducamBackLeft.getCameraMatrix(); @@ -617,12 +619,12 @@ public void periodic() { arducamBackLeftDistCoeffs = arducamBackLeft.getDistCoeffs(); } - // if (arducamBackRightMatrix.isEmpty()) { - // arducamBackRightMatrix = arducamBackRight.getCameraMatrix(); - // } - // if (arducamBackRightDistCoeffs.isEmpty()) { - // arducamBackRightDistCoeffs = arducamBackRight.getDistCoeffs(); - // } + if (arducamBackRightMatrix.isEmpty()) { + arducamBackRightMatrix = arducamBackRight.getCameraMatrix(); + } + if (arducamBackRightDistCoeffs.isEmpty()) { + arducamBackRightDistCoeffs = arducamBackRight.getDistCoeffs(); + } // if (arducamFuelMatrix.isEmpty()) { // arducamFuelMatrix = arducamFuel.getCameraMatrix(); @@ -818,25 +820,25 @@ private void updateVisionPoseEstimates() { detectedTargets.clear(); rejectedPoses.clear(); - // updateVisionPoses( - // latestArducamLeftResult, - // leftPoseEstimator, - // arducamLeftMatrix, - // arducamLeftDistCoeffs, - // VisionConstants.arducamLeftTransform, - // Units.inchesToMeters(3.0), - // Units.inchesToMeters(2.5), - // 1); - - // updateVisionPoses( - // latestArducamRightResult, - // rightPoseEstimator, - // arducamRightMatrix, - // arducamRightDistCoeffs, - // VisionConstants.arducamRightTransform, - // Units.inchesToMeters(3.0), - // Units.inchesToMeters(2.5), - // 1); + updateVisionPoses( + latestArducamLeftResult, + leftPoseEstimator, + arducamLeftMatrix, + arducamLeftDistCoeffs, + VisionConstants.arducamLeftTransform, + Units.inchesToMeters(3.0), + Units.inchesToMeters(2.5), + 1); + + updateVisionPoses( + latestArducamRightResult, + rightPoseEstimator, + arducamRightMatrix, + arducamRightDistCoeffs, + VisionConstants.arducamRightTransform, + Units.inchesToMeters(3.0), + Units.inchesToMeters(2.5), + 1); updateVisionPoses( latestArducamBackLeftResult, @@ -848,15 +850,15 @@ private void updateVisionPoseEstimates() { Units.inchesToMeters(2.5), 1); - // updateVisionPoses( - // latestArducamBackRightResult, - // backRightPoseEstimator, - // arducamBackRightMatrix, - // arducamBackRightDistCoeffs, - // VisionConstants.arducamBackRightTransform, - // Units.inchesToMeters(3.0), - // Units.inchesToMeters(2.5), - // 1); + updateVisionPoses( + latestArducamBackRightResult, + backRightPoseEstimator, + arducamBackRightMatrix, + arducamBackRightDistCoeffs, + VisionConstants.arducamBackRightTransform, + Units.inchesToMeters(3.0), + Units.inchesToMeters(2.5), + 1); Collections.sort(poseEstimates); @@ -1055,33 +1057,31 @@ private void initVisionSim() { .setLatencyStdDevMs(15) .setExposureTimeMs(45); - // arducamLeftSim = - // new PhotonCameraSim(arducamLeft, arducamProperties, FieldConstants.aprilTagLayout); - // arducamRightSim = - // new PhotonCameraSim(arducamRight, arducamProperties, FieldConstants.aprilTagLayout); + arducamLeftSim = + new PhotonCameraSim(arducamLeft, arducamProperties, FieldConstants.aprilTagLayout); + arducamRightSim = + new PhotonCameraSim(arducamRight, arducamProperties, FieldConstants.aprilTagLayout); arducamBackLeftSim = new PhotonCameraSim(arducamBackLeft, arducamProperties, FieldConstants.aprilTagLayout); - // arducamBackRightSim = - // new PhotonCameraSim(arducamBackRight, arducamProperties, FieldConstants.aprilTagLayout); + arducamBackRightSim = + new PhotonCameraSim(arducamBackRight, arducamProperties, FieldConstants.aprilTagLayout); // arducamFuelSim = // new PhotonCameraSim(arducamFuel, arducamProperties, FieldConstants.aprilTagLayout); - // visionSim.addCamera(arducamLeftSim, VisionConstants.arducamLeftTransform); - // visionSim.addCamera(arducamRightSim, VisionConstants.arducamRightTransform); + visionSim.addCamera(arducamLeftSim, VisionConstants.arducamLeftTransform); + visionSim.addCamera(arducamRightSim, VisionConstants.arducamRightTransform); visionSim.addCamera(arducamBackLeftSim, VisionConstants.arducamBackLeftTransform); - // visionSim.addCamera(arducamBackRightSim, VisionConstants.arducamBackRightTransform); + visionSim.addCamera(arducamBackRightSim, VisionConstants.arducamBackRightTransform); // visionSim.addCamera(arducamFuelSim, VisionConstants.arducamFuelTransform); - // visionSim.addCamera(arducamFrontSim, VisionConstants.arducamFrontTransform); - // arducamLeftSim.enableRawStream(true); // arducamLeftSim.enableProcessedStream(true); // arducamRightSim.enableRawStream(true); // arducamRightSim.enableProcessedStream(true); - arducamBackLeftSim.enableRawStream(true); - arducamBackLeftSim.enableProcessedStream(true); + // arducamBackLeftSim.enableRawStream(true); + // arducamBackLeftSim.enableProcessedStream(true); // arducamBackRightSim.enableRawStream(true); // arducamBackRightSim.enableProcessedStream(true); @@ -1182,15 +1182,6 @@ public SwerveModuleState[] getDesiredStates() { return stateCache.ModuleTargets; } - private PathPlannerPath loadPath(String pathName) { - try { - // return PathPlannerPath.fromChoreoTrajectory(pathName); - return PathPlannerPath.fromPathFile(pathName); - } catch (Exception e) { - return null; - } - } - public PathPlannerPath nearestPath(Pose2d referencePose, Collection paths) { return Collections.min( paths, diff --git a/src/main/java/frc/robot/subsystems/Turret.java b/src/main/java/frc/robot/subsystems/Turret.java index 8753f78..77f7ffa 100644 --- a/src/main/java/frc/robot/subsystems/Turret.java +++ b/src/main/java/frc/robot/subsystems/Turret.java @@ -224,7 +224,7 @@ public boolean hasDriftedTooMuch(Angle tolerance) { @Logged(name = "Zeroed Poses Turret") public Pose3d[] zeroedComponentPoses() { - return new Pose3d[] {new Pose3d(), new Pose3d()}; + return new Pose3d[] {new Pose3d(), new Pose3d(), new Pose3d(), new Pose3d()}; } @Override diff --git a/src/main/java/frc/robot/util/FuelSim.java b/src/main/java/frc/robot/util/FuelSim.java index 1771cab..4e2d11f 100644 --- a/src/main/java/frc/robot/util/FuelSim.java +++ b/src/main/java/frc/robot/util/FuelSim.java @@ -725,7 +725,7 @@ public static class Hub { private final int exitVelXMult; private int score = 0; - private boolean onlyScoreWhenActive = false; + private boolean onlyCountWhenActive = false; private Hub(Translation2d center, Translation3d exit, int exitVelXMult) { this.center = center; @@ -733,15 +733,15 @@ private Hub(Translation2d center, Translation3d exit, int exitVelXMult) { this.exitVelXMult = exitVelXMult; } - public void toggleScoreWhenActive(boolean toggle) { - onlyScoreWhenActive = toggle; + public void toggleCountWhenActive(boolean toggle) { + onlyCountWhenActive = toggle; } private void handleHubInteraction(Fuel fuel) { if (didFuelScore(fuel)) { fuel.pos = exit; fuel.vel = getDispersalVelocity(); - if (onlyScoreWhenActive) { + if (onlyCountWhenActive) { if (HubTracker.isActive()) score++; } else { score++; diff --git a/src/main/java/frc/robot/util/RobotVisualization.java b/src/main/java/frc/robot/util/RobotVisualization.java index d2b2147..8e29c4c 100644 --- a/src/main/java/frc/robot/util/RobotVisualization.java +++ b/src/main/java/frc/robot/util/RobotVisualization.java @@ -1,6 +1,7 @@ package frc.robot.util; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; import edu.wpi.first.epilogue.Logged; @@ -16,10 +17,12 @@ import frc.robot.Constants.HoodConstants; import frc.robot.Constants.SimConstants; import frc.robot.Constants.TurretConstants; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Hood; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Turret; +import frc.robot.util.SOTMCalculator.ShootingParameters; import java.util.ArrayList; import java.util.List; @@ -28,16 +31,19 @@ public class RobotVisualization { private Hood hood; private Swerve swerve; private Shooter shooter; + private Climber climber; private static int fuelStored = 8; private static List activeShots = new ArrayList<>(); - public RobotVisualization(Turret turret, Hood hood, Swerve swerve, Shooter shooter) { + public RobotVisualization( + Turret turret, Hood hood, Swerve swerve, Shooter shooter, Climber climber) { this.turret = turret; this.hood = hood; this.swerve = swerve; this.shooter = shooter; + this.climber = climber; } @Logged(name = "Turret") @@ -55,6 +61,15 @@ public Pose3d getHoodPose3d() { new Rotation3d(0.0, hood.getHoodAngle().in(Radians), 0.0))); } + @Logged(name = "Climber Poses") + public Pose3d[] getClimberPose3d() { + return new Pose3d[] { + new Pose3d( + -0.2861912, 0.0635, 0.104775 + climber.getClimberHeight().in(Meters), Rotation3d.kZero), + new Pose3d(-0.2861912, 0.0635, 0.104775, Rotation3d.kZero) + }; + } + @Logged(name = "Fuel Stored") public int getFuelStored() { return fuelStored; @@ -93,7 +108,7 @@ public Command shootFuelGatherData() { .withName("ShootFuelGatherData"); } - public void shootFuel() { + public void shootFuel(ShootingParameters shootingParameters) { if (RobotBase.isReal()) return; if (fuelStored < 1) return; @@ -110,7 +125,7 @@ public void shootFuel() { // once turret is tuned better FuelSim.getInstance() .launchFuel( - shooter.angularToLinearVelocity(shooter.getCurrentVelocity()), + shootingParameters.shooterSpeed(), Radians.of(Math.PI / 2).minus(hood.getHoodAngle()), swerve.getRobotPose().getRotation().getMeasure().plus(turret.getTurretAngle()), TurretConstants.robotToTurret); diff --git a/src/main/java/frc/robot/util/SOTMCalculator.java b/src/main/java/frc/robot/util/SOTMCalculator.java index 6745c38..51e9d70 100644 --- a/src/main/java/frc/robot/util/SOTMCalculator.java +++ b/src/main/java/frc/robot/util/SOTMCalculator.java @@ -14,17 +14,19 @@ import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.SOTMConstants; +import frc.robot.Constants.SimConstants; import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Turret; public class SOTMCalculator { - public static InterpolatingTreeMap hoodAngleMap = SOTMConstants.hoodAngleMap; - public static InterpolatingDoubleTreeMap shooterSpeedMap = SOTMConstants.shooterSpeedMap; - public static InterpolatingDoubleTreeMap timeOfFlightMap = SOTMConstants.timeOfFlightMap; + // public static InterpolatingTreeMap hoodAngleMap = + // SOTMConstants.hoodAngleMap; + // public static InterpolatingDoubleTreeMap shooterSpeedMap = SOTMConstants.shooterSpeedMap; + // public static InterpolatingDoubleTreeMap timeOfFlightMap = SOTMConstants.timeOfFlightMap; private static Translation2d robotToTurret2d = TurretConstants.robotToTurret.toTranslation2d(); - public static Time accelTime = Seconds.of(0.1353); +// public static Time accelTime = Seconds.of(SimConstants.loopPeriodSecs); public record ShootingParameters( LinearVelocity shooterSpeed, @@ -38,9 +40,17 @@ public static ShootingParameters getParameters( Pose2d target, double fieldAccelX, double fieldAccelY, - ChassisSpeeds fieldChassisSpeeds) { + ChassisSpeeds fieldChassisSpeeds, + boolean isScoring) { Pose2d targetPose = target; + InterpolatingTreeMap hoodAngleMap = + isScoring ? SOTMConstants.hoodAngleMapScoring : SOTMConstants.hoodAngleMapFerrying; + InterpolatingDoubleTreeMap shooterSpeedMap = + isScoring ? SOTMConstants.shooterSpeedMapScoring : SOTMConstants.shooterSpeedMapFerrying; + InterpolatingDoubleTreeMap timeOfFlightMap = + isScoring ? SOTMConstants.timeOfFlightMapScoring : SOTMConstants.timeOfFlightMapFerrying; + Pose2d robotPose = swerve.getRobotPose(); Translation2d turretPose = diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index afac990..fbb5c80 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.2.1", + "version": "v2026.2.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.2.1", + "version": "v2026.2.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.2.1", + "version": "v2026.2.2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.2.1", + "version": "v2026.2.2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.2.1" + "version": "v2026.2.2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.2.1" + "version": "v2026.2.2" } ] } \ No newline at end of file