diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSide2ScoreN4.auto b/src/main/deploy/pathplanner/autos/VisionSourceSide2ScoreN4.auto new file mode 100644 index 00000000..beaf7e94 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionSourceSide2ScoreN4.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 4.47 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowSpeakerToFarN4ToLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4ToLaunch.path b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4ToLaunch.path new file mode 100644 index 00000000..dd5390e3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4ToLaunch.path @@ -0,0 +1,144 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7275269251393202, + "y": 4.4671669970120575 + }, + "prevControl": null, + "nextControl": { + "x": 1.146100930190991, + "y": 3.4001885029736156 + }, + "isLocked": false, + "linkedName": "LowSpeaker" + }, + { + "anchor": { + "x": 3.7844624260677096, + "y": 1.7444754889509784 + }, + "prevControl": { + "x": 2.076817112087623, + "y": 1.7678637978358573 + }, + "nextControl": { + "x": 4.556702651817908, + "y": 1.7338987065329259 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.44590590082081, + "y": 2.4960698570045796 + }, + "prevControl": { + "x": 7.976085063097927, + "y": 2.378058890871132 + }, + "nextControl": { + "x": 9.030376579271676, + "y": 2.642878903991426 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.690191951812475, + "y": 1.7444754889509784 + }, + "prevControl": { + "x": 7.053385240517052, + "y": 1.8806304206600526 + }, + "nextControl": { + "x": 4.326998663107897, + "y": 1.6083205572419041 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9089022069531514, + "y": 1.9539173222406319 + }, + "prevControl": { + "x": 3.9750353786421866, + "y": 1.6125403679378794 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceVisionLaunch" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.05, + "rotationDegrees": 120.0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": -171.13373619481098, + "rotateFast": false + }, + { + "waypointRelativePos": 1.75, + "rotationDegrees": -167.33444776423735, + "rotateFast": false + }, + { + "waypointRelativePos": 4.0, + "rotationDegrees": -164.3286881943862, + "rotateFast": false + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 159.12457737382582, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 1.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 120.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index 8b88ec01..d3feefb6 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -11,7 +11,7 @@ }, "conversionFactor": { "angle": 19.2, - "drive": 0.0696793458626792 + "drive": 0.06658958893604663608562691131498 }, "rampRate": { "drive": 0.25, diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index c137e3d9..16e7c4fc 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -2,6 +2,7 @@ import static frc.team2412.robot.Controls.ControlConstants.CODRIVER_CONTROLLER_PORT; import static frc.team2412.robot.Controls.ControlConstants.CONTROLLER_PORT; +import static frc.team2412.robot.Subsystems.SubsystemConstants.APRILTAGS_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.LAUNCHER_ENABLED; @@ -11,6 +12,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -136,6 +138,25 @@ public Controls(Subsystems s) { // this, // codriveController.leftBumper())); } + if (DRIVEBASE_ENABLED && APRILTAGS_ENABLED) { + new Trigger(s.rotateToSpeaker) + .whileTrue( + s.drivebaseSubsystem + .rotateToAngle( + () -> { + double currentTimestamp = Timer.getFPGATimestamp(); + double robotToTagAngleTimestamp = + s.apriltagsProcessor.getLastRotatedAngleTimestamp(); + var robotAngle = s.drivebaseSubsystem.getPose().getRotation(); + var robotToTagAngle = s.apriltagsProcessor.getLastRotatedAngle(); + if (currentTimestamp - robotToTagAngleTimestamp > 0.1) { + return robotAngle; + } + return robotAngle.plus(robotToTagAngle); + }, + false) + .withName("RotateToSpeaker")); + } } // LED private void bindLEDControls() { @@ -211,12 +232,14 @@ private void bindLauncherControls() { launcherLowerPresetButton.onTrue( s.launcherSubsystem .runOnce(s.launcherSubsystem::stopLauncher) - .andThen(new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE))); + .andThen(new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE)) + .withTimeout(2.0)); launcherSubwooferPresetButton.onTrue( new SetAngleLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.SUBWOOFER_AIM_ANGLE)); + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.SUBWOOFER_AIM_ANGLE) + .withTimeout(2.0)); // launcherPodiumPresetButton.onTrue( // new SetAngleLaunchCommand( // s.launcherSubsystem, @@ -224,9 +247,10 @@ private void bindLauncherControls() { // LauncherSubsystem.PODIUM_AIM_ANGLE)); launcherAmpPresetButton.onTrue( new SetAngleAmpLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.AMP_AIM_ANGLE)); + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.AMP_AIM_ANGLE) + .withTimeout(2.0)); // launcherTrapPresetButton.onTrue( // TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); @@ -244,7 +268,7 @@ private void bindLauncherControls() { // s.launcherSubsystem.runEnd( // s.launcherSubsystem::launch, s.launcherSubsystem::stopLauncher)); - driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch(6500))); + driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch())); } private void bindSysIdControls() { diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index adaf6840..d3b7ea91 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -9,6 +9,7 @@ import frc.team2412.robot.subsystems.LauncherSubsystem; import frc.team2412.robot.subsystems.LimelightSubsystem; import frc.team2412.robot.util.DrivebaseWrapper; +import java.util.function.BooleanSupplier; public class Subsystems { public static class SubsystemConstants { @@ -29,6 +30,7 @@ public static class SubsystemConstants { public final IntakeSubsystem intakeSubsystem; public final LEDSubsystem ledSubsystem; public final AprilTagsProcessor apriltagsProcessor; + public final BooleanSupplier rotateToSpeaker; public Subsystems() { // initialize subsystems here (wow thats wild) @@ -41,8 +43,10 @@ public Subsystems() { } if (APRILTAGS_ENABLED) { apriltagsProcessor = new AprilTagsProcessor(drivebaseWrapper); + rotateToSpeaker = () -> apriltagsProcessor.shouldRotateToSpeaker(); } else { apriltagsProcessor = null; + rotateToSpeaker = () -> false; } if (LAUNCHER_ENABLED) { launcherSubsystem = new LauncherSubsystem(); diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index 3aa74044..d5df1d44 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -11,8 +11,10 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -45,7 +47,7 @@ public class AprilTagsProcessor { Units.inchesToMeters(27.0 / 2.0 - 0.94996), 0, Units.inchesToMeters(8.12331), - new Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(-30), 0)); + new Rotation3d(0, Units.degreesToRadians(-30), 0)); // TODO Measure these private static final Vector STANDARD_DEVS = @@ -81,11 +83,18 @@ private static boolean resultIsValid(PhotonPipelineResult result) { private final DrivebaseWrapper aprilTagsHelper; private final FieldObject2d rawVisionFieldObject; + private final GenericEntry rotateToSpeakerEntry; + private final GenericEntry trackingIdEntry; + // These are always set with every pipeline result private double lastRawTimestampSeconds = 0; private PhotonPipelineResult latestResult = null; private boolean latestResultIsValid = false; + // These are set with every pipeline result with the appropriate tag + private Rotation2d lastRotatedAngle = new Rotation2d(); + private double lastRotatedAngleTimestamp = 0; + // This is set for every non-filtered pipeline result private Optional latestPose = Optional.empty(); @@ -96,6 +105,9 @@ private static boolean resultIsValid(PhotonPipelineResult result) { private static final AprilTagFieldLayout fieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + public static final Pose2d RED_SPEAKER_POSE = fieldLayout.getTagPose(4).get().toPose2d(); + public static final Pose2d BLUE_SPEAKER_POSE = fieldLayout.getTagPose(7).get().toPose2d(); + public AprilTagsProcessor(DrivebaseWrapper aprilTagsHelper) { this.aprilTagsHelper = aprilTagsHelper; rawVisionFieldObject = aprilTagsHelper.getField().getObject("RawVision"); @@ -143,12 +155,29 @@ public AprilTagsProcessor(DrivebaseWrapper aprilTagsHelper) { .add("3d pose on field", new SendablePose3d(this::getRobotPose)) .withPosition(2, 0) .withSize(2, 2); + rotateToSpeakerEntry = + shuffleboardTab + .add("Rotate to speaker tag", false) + .withPosition(0, 2) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kToggleSwitch) + .getEntry(); + trackingIdEntry = shuffleboardTab.add("Tag ID", 4).withPosition(1, 2).withSize(1, 1).getEntry(); + shuffleboardTab + .addDouble("Last rotated angle timestamp", this::getLastRotatedAngleTimestamp) + .withPosition(2, 2) + .withSize(1, 1); + shuffleboardTab + .addDouble("Last rotated angle degrees", () -> getLastRotatedAngle().getDegrees()) + .withPosition(3, 2) + .withSize(1, 1); } public void update() { latestResult = photonCamera.getLatestResult(); latestResultIsValid = resultIsValid(latestResult); lastRawTimestampSeconds = latestResult.getTimestampSeconds(); + updateSpeakerAngle(latestResult); if (!latestResultIsValid) { return; } @@ -164,6 +193,35 @@ public void update() { } } + private void updateSpeakerAngle(PhotonPipelineResult pipelineResult) { + int trackingId = (int) trackingIdEntry.getInteger(-1); + for (var target : pipelineResult.targets) { + if (target.getFiducialId() != trackingId) { + continue; + } + // Camera is rotated so that global left is camera down + lastRotatedAngle = Rotation2d.fromDegrees(-target.getPitch()); + lastRotatedAngleTimestamp = pipelineResult.getTimestampSeconds(); + } + } + + /** + * Indicates if the rotate to speaker toggle is selected. + * + * @return Whether the rotate to speaker toggle is selected. + */ + public boolean shouldRotateToSpeaker() { + return rotateToSpeakerEntry.getBoolean(false); + } + + public Rotation2d getLastRotatedAngle() { + return lastRotatedAngle; + } + + public double getLastRotatedAngleTimestamp() { + return lastRotatedAngleTimestamp; + } + /** * Returns the timestamp of the last result we got (regardless of whether it has any valid * targets) diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 9c54c813..5bd5fb96 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -196,32 +196,34 @@ public Command driveJoystick( Supplier rotation, BooleanSupplier turboRotation) { return this.run( - () -> { - Rotation2d constrainedRotation = - Rotation2d.fromRotations( - SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND) - * MAX_SPEED - * (turboRotation.getAsBoolean() - ? turboRotationMultiplierEntry.getDouble(1.0) - : 1) - * rotationSpeedEntry.getDouble(1.0) - * -1); - Translation2d constrainedTranslation = - new Translation2d( - SwerveMath.applyDeadband(forward.getAsDouble(), true, JOYSTICK_DEADBAND) - * MAX_SPEED - * translationSpeedEntry.getDouble(1.0), - SwerveMath.applyDeadband(strafe.getAsDouble(), true, JOYSTICK_DEADBAND) - * MAX_SPEED - * translationSpeedEntry.getDouble(1.0)); - if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { - constrainedTranslation = constrainedTranslation.unaryMinus(); - } - if (flipTranslationEntry.getBoolean(false)) { - constrainedTranslation = constrainedTranslation.unaryMinus(); - } - drive(constrainedTranslation, constrainedRotation, true); - }); + () -> { + Rotation2d constrainedRotation = + Rotation2d.fromRotations( + SwerveMath.applyDeadband( + rotation.get().getRotations(), true, JOYSTICK_DEADBAND) + * MAX_SPEED + * (turboRotation.getAsBoolean() + ? turboRotationMultiplierEntry.getDouble(1.0) + : 1) + * rotationSpeedEntry.getDouble(1.0) + * -1); + Translation2d constrainedTranslation = + new Translation2d( + SwerveMath.applyDeadband(forward.getAsDouble(), true, JOYSTICK_DEADBAND) + * MAX_SPEED + * translationSpeedEntry.getDouble(1.0), + SwerveMath.applyDeadband(strafe.getAsDouble(), true, JOYSTICK_DEADBAND) + * MAX_SPEED + * translationSpeedEntry.getDouble(1.0)); + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { + constrainedTranslation = constrainedTranslation.unaryMinus(); + } + if (flipTranslationEntry.getBoolean(false)) { + constrainedTranslation = constrainedTranslation.unaryMinus(); + } + drive(constrainedTranslation, constrainedRotation, true); + }) + .withName("DriveJoystick"); } // this might need to be put in its own file due to complexity @@ -310,7 +312,7 @@ private void initShuffleboard() { headingCorrectionEntry = drivebaseTab - .addPersistent("Heading Correction", true) + .add("Heading Correction", false) .withWidget(BuiltInWidgets.kToggleSwitch) .withSize(2, 1) .getEntry(); @@ -323,28 +325,28 @@ private void initShuffleboard() { translationSpeedEntry = drivebaseTab - .addPersistent("Translation Speed", 1.0) + .add("Translation Speed", 1.0) .withWidget(BuiltInWidgets.kNumberSlider) .withSize(2, 1) .withProperties(Map.of("Min", 0.0)) .getEntry(); rotationSpeedEntry = drivebaseTab - .addPersistent("Rotation Speed", 1.0) + .add("Rotation Speed", 0.38) .withWidget(BuiltInWidgets.kNumberSlider) .withSize(2, 1) .withProperties(Map.of("Min", 0.0)) .getEntry(); turboRotationMultiplierEntry = drivebaseTab - .addPersistent("Turbo rotation multiplier", 1.0) + .add("Turbo rotation multiplier", 1.75) .withWidget(BuiltInWidgets.kNumberSlider) .withSize(2, 1) .withProperties(Map.of("Min", 0.5, "Max", 5.0)) .getEntry(); xWheelsEntry = drivebaseTab - .addPersistent("X Wheels", xWheelsEnabled) + .add("X Wheels", false) .withWidget(BuiltInWidgets.kBooleanBox) .withSize(1, 1) .getEntry(); diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index ec0da9c4..20288701 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -307,7 +307,7 @@ public boolean debouncedFeederSensor() { boolean feederSensorSignal = feederSensorDebouncer.calculate(feederSensor.isPressed()); boolean feederSensorIRSignal = feederSensorIRDebouncer.calculate(!feederSensorIR.get()); - return feederSensorSignal || feederSensorIRSignal; + return feederSensorSignal; } public boolean feederSensorHasNote() { diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 31d5871b..5c040f00 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -5,6 +5,7 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; @@ -17,6 +18,7 @@ import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; @@ -29,6 +31,7 @@ import frc.team2412.robot.util.SparkPIDWidget; import java.util.Map; import java.util.Optional; +import java.util.OptionalDouble; public class LauncherSubsystem extends SubsystemBase { // CONSTANTS @@ -102,6 +105,7 @@ public class LauncherSubsystem extends SubsystemBase { private double angleSetpoint; private double manualAngleSetpoint; private boolean ignoreLimits; + private boolean badCAN; private Optional relativeEncoderStartPosition; @@ -127,6 +131,8 @@ public class LauncherSubsystem extends SubsystemBase { private GenericEntry launcherDisabledEntry; + private OptionalDouble angleInsaneStartTime = OptionalDouble.empty(); + // Constructors public LauncherSubsystem() { @@ -252,6 +258,12 @@ public void launch() { } // used for presets public void launch(double speed) { + if (badCAN) { + rpmSetpoint = 0; + launcherTopMotor.stopMotor(); + launcherBottomMotor.stopMotor(); + return; + } rpmSetpoint = speed; launcherTopPIDController.setReference( @@ -261,6 +273,10 @@ public void launch(double speed) { } public void ampLaunch(double speed) { + if (badCAN) { + launcherTopMotor.stopMotor(); + return; + } launcherTopPIDController.setReference( -speed, ControlType.kVelocity, 0); // launcherTopFeedforward.calculate(-speed)); launcherBottomMotor.disable(); @@ -285,9 +301,20 @@ public double getAngle() { public double getPosition() { if (!USE_THROUGHBORE) { - return launcherAngleEncoder.getPosition(); + double position = launcherAngleEncoder.getPosition(); + badCAN = launcherAngleOneMotor.getLastError() != REVLibError.kOk; + if (badCAN) { + launcherAngleOneMotor.stopMotor(); + } + return position; } - return convertEncoderRotationsToPivotRotations(launcherAngleThroughboreEncoder.getPosition()); + double position = + convertEncoderRotationsToPivotRotations(launcherAngleThroughboreEncoder.getPosition()); + badCAN = launcherAngleTwoMotor.getLastError() != REVLibError.kOk; + if (badCAN) { + launcherAngleTwoMotor.stopMotor(); + } + return position; } /** @@ -473,7 +500,7 @@ private void initShuffleboard() { setAngleOffsetEntry = Shuffleboard.getTab("Match") - .add("Set Angle Offset", 0) + .add("Set Angle Offset", -5) .withPosition(4, 3) .withSize(2, 1) .withWidget(BuiltInWidgets.kNumberSlider) @@ -530,6 +557,10 @@ public void resetManualAngleSetpoint() { manualAngleSetpoint = Units.degreesToRotations(getAngle()); } + public boolean hasBadCAN() { + return badCAN; + } + @Override public void periodic() { launcherAngleEntry.setDouble(getAngle()); @@ -539,16 +570,22 @@ public void periodic() { angleSetpointEntry.setDouble(angleSetpoint); launcherFlywheelSetpointEntry.setDouble(rpmSetpoint); launcherDisabledEntry.setBoolean(false); + double now = Timer.getFPGATimestamp(); // PIVOT ENCODER SANITY CHECKS // compares the relative encoder angle vs the absolute encoder angle if (relativeEncoderStartPosition.isPresent() && Math.abs(getAngle() - getAngleOneMotorAngle()) >= ENCODER_DIFFERENCE_TOLERANCE) { - if (!ignoreLimits) { + if (!ignoreLimits + && angleInsaneStartTime.isPresent() + && now - angleInsaneStartTime.getAsDouble() > 0.15) { launcherAngleOneMotor.disable(); launcherAngleTwoMotor.disable(); launcherDisabledEntry.setBoolean(true); } + if (angleInsaneStartTime.isEmpty()) { + angleInsaneStartTime = OptionalDouble.of(now); + } DriverStation.reportError( "Pivot encoder deviated too far from motor encoder angle ... .. Reported pivot angle of " + getAngle() diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index 56e39318..c9578282 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -17,7 +17,10 @@ public class MatchDashboard { public MatchDashboard(Subsystems s) { field = s.drivebaseWrapper.getField(); - tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1); + tab.add(new FMSWidget()).withPosition(0, 0).withSize(3, 1); + tab.addBoolean("Bad launcher CAN", s.launcherSubsystem::hasBadCAN) + .withPosition(3, 0) + .withSize(1, 1); tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); AutonomousField.configureShuffleboardTab(tab, 6, 0, "Available Auto Variants", r::addPeriodic); diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 9df1141e..e8f5e85f 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -105,9 +105,9 @@ public static enum StartPosition { new AutoPath("Autoline N1", "PresetAmpSide2Score"), new AutoPath("Autoline N2", "PresetMidAutoline2Score"), new AutoPath("Autoline N3", "PresetSourceSideAutoline2Score"), - new AutoPath("Centerline N5", "PresetSourceSideFar2Score") + new AutoPath("Centerline N5", "PresetSourceSideFar2Score"), // vision - ); + new AutoPath("Centerline N4", "VisionSourceSide2ScoreN4", true)); private static List twoPiecePaths = List.of( diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index a6add886..1dc75453 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -414,7 +414,8 @@ public void drive( if (headingCorrection) { if (Math.abs(getRobotVelocity().omegaRadiansPerSecond) < HEADING_CORRECTION_ROTATION_DEADBAND && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_TRANSLATION_DEADBAND - || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_TRANSLATION_DEADBAND)) { + || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_TRANSLATION_DEADBAND) + && (Math.abs(velocity.omegaRadiansPerSecond) < 0.01)) { velocity.omegaRadiansPerSecond = swerveController.headingCalculate( getOdometryHeading().getRadians(), lastHeadingRadians); diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 77641e45..0e80a16c 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.9", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.9", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.9", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.9" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.9" + "version": "v2024.3.1" } ] } \ No newline at end of file