diff --git a/shuffleboard.json b/shuffleboard.json index 5c50475..eb024c0 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -138,6 +138,38 @@ "_glyph": 148, "_showGlyph": false } + }, + "4,1": { + "size": [ + 4, + 2 + ], + "content": { + "_type": "Field", + "_source0": "network_table:///SmartDashboard/VisionSystemSim-frontCam/Sim Field", + "_title": "VisionSystemSim-frontCam/Sim Field", + "_glyph": 148, + "_showGlyph": false, + "Game/Game": "k2024Crescendo", + "Visuals/Robot Icon Size": 50.0, + "Visuals/Show Outside Circles": false, + "Colors/cameras": "#FFFFFFFF", + "Colors/visibleTargetPoses": "#FFFFFFFF", + "Colors/apriltag": "#FFFFFFFF" + } + }, + "4,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Command", + "_source0": "network_table:///SmartDashboard/Zero Gyro", + "_title": "Zero Gyro", + "_glyph": 148, + "_showGlyph": false + } } } } @@ -157,9 +189,9 @@ } ], "windowGeometry": { - "x": 1378.0, - "y": 353.0, - "width": 2574.0, - "height": 568.0 + "x": 1040.0, + "y": 382.0, + "width": 1230.0, + "height": 565.0 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 190b4c5..e077eb8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -271,7 +271,7 @@ private void configureDriverControllerBindings() { .runOnce( () -> { headingController.reset(); - headingController.setGoal(angle.getRadians()); + headingController.setGoal(angle); }) .withName(String.format("PrepareLockedHeading %s", name))); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 51424bf..6233007 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -343,15 +343,16 @@ public void setRobotSpeeds(ChassisSpeeds speeds, boolean fieldRelative) { currentSetpoint = setpointGenerator.generateSetpoint( currentModuleLimits, currentSetpoint, speeds, Constants.LOOP_PERIOD_SECONDS); - SwerveDriveWheelStates cheesyWheelSpeeds = + SwerveDriveWheelStates swerveGenerator = new SwerveDriveWheelStates(currentSetpoint.moduleStates()); - Logger.recordOutput("SwerveStates/254/CheesyPoofsDesiredWheelSpeeds", cheesyWheelSpeeds.states); + Logger.recordOutput( + "SwerveStates/swerveGen/CheesyPoofsDesiredWheelSpeeds", swerveGenerator.states); speeds = ChassisSpeeds.discretize(speeds, Constants.LOOP_PERIOD_SECONDS); SwerveDriveWheelStates wheelSpeeds = kinematics.toWheelSpeeds(speeds); - Logger.recordOutput("SwerveStates/254/DefaultDesiredWheelSpeeds", wheelSpeeds.states); + Logger.recordOutput("SwerveStates/swerveGen/DefaultDesiredWheelSpeeds", wheelSpeeds.states); - setWheelSpeeds(DriveConstants.USE_254_SWERVE_SETPOINT ? cheesyWheelSpeeds : wheelSpeeds); + setWheelSpeeds(DriveConstants.USE_SWERVE_SETPOINT_GENERATOR ? swerveGenerator : wheelSpeeds); } // --- Wheel States --- diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index e255a0c..258a483 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -14,7 +14,7 @@ */ public class DriveConstants { - public static final boolean USE_254_SWERVE_SETPOINT = true; + public static final boolean USE_SWERVE_SETPOINT_GENERATOR = true; // --- Drive Config --- @@ -141,6 +141,12 @@ public record ModuleConstants( new PIDConstants(10.0, 0.0, 0.0), Mk4Reductions.L1.reduction, Mk4Reductions.TURN.reduction); + case SIM_BOT -> new ModuleConstants( + new FeedForwardConstants(0.1, 3.12, 0.40), + new PIDConstants(0.1, 0.0, 0.0), + new PIDConstants(10.0, 0.0, 0.0), + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); default -> new ModuleConstants( new FeedForwardConstants(0.1, 2.35, 0.53), new PIDConstants(0.1, 0.0, 0.0), diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index b22a641..a0b75e7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -34,9 +34,9 @@ public class Module { group.build("DriveKd", MODULE_CONSTANTS.driveFeedback().kD()); private static final LoggedTunableNumber turnKp = - group.build("TurnKp", MODULE_CONSTANTS.driveFeedback().kP()); + group.build("TurnKp", MODULE_CONSTANTS.turnFeedback().kP()); private static final LoggedTunableNumber turnKd = - group.build("TurnKd", MODULE_CONSTANTS.driveFeedback().kD()); + group.build("TurnKd", MODULE_CONSTANTS.turnFeedback().kD()); private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); diff --git a/src/main/java/frc/robot/subsystems/drive/controllers/HeadingController.java b/src/main/java/frc/robot/subsystems/drive/controllers/HeadingController.java index b9aaf3c..28a8d16 100644 --- a/src/main/java/frc/robot/subsystems/drive/controllers/HeadingController.java +++ b/src/main/java/frc/robot/subsystems/drive/controllers/HeadingController.java @@ -3,6 +3,7 @@ import static frc.robot.subsystems.drive.DriveConstants.HEADING_CONTROLLER_CONSTANTS; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import frc.robot.Constants; @@ -64,16 +65,16 @@ public void reset() { } /** - * Set goal heading in radians. Calculate will now give values to get to this heading. + * Set goal heading. Calculate will now give values to get to this heading. * - * @param headingRadians desired heading of chassis in radians + * @param headingRadians desired heading of chassis */ - public void setGoal(double headingRadians) { - headingControllerRadians.setGoal(headingRadians); + public void setGoal(Rotation2d headingRadians) { + headingControllerRadians.setGoal(headingRadians.getRadians()); } /** - * Get goal heading in radians. + * Get goal heading. * * @return desired heading of chassis */ @@ -84,10 +85,10 @@ public double getGoal() { /** * Get speed chassis needs to rotation at to reach heading goal * - * @param goalHeadingRadians desired heading of chassis in radians + * @param goalHeadingRadians desired heading of chassis * @return rotation speed to reach heading goal, omega radians per second */ - public double calculate(double goalHeadingRadians) { + public double calculate(Rotation2d goalHeadingRadians) { setGoal(goalHeadingRadians); return calculate(); } diff --git a/src/main/java/frc/robot/subsystems/drive/controllers/TeleopDriveController.java b/src/main/java/frc/robot/subsystems/drive/controllers/TeleopDriveController.java index e1b36be..536541b 100644 --- a/src/main/java/frc/robot/subsystems/drive/controllers/TeleopDriveController.java +++ b/src/main/java/frc/robot/subsystems/drive/controllers/TeleopDriveController.java @@ -9,6 +9,8 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.utility.LoggedTunableNumber; import frc.robot.utility.LoggedTunableNumberGroup; + +import java.util.Optional; import java.util.function.DoubleSupplier; /** Controller for converting joystick values to drive components */ @@ -77,8 +79,8 @@ public Rotation2d getOmegaRadiansPerSecond() { * * @return heading angle */ - public Rotation2d getHeadingDirection() { - if (DriverStation.isAutonomous()) return new Rotation2d(); + public Optional getHeadingDirection() { + if (DriverStation.isAutonomous()) return Optional.empty(); return TeleopDriveController.getHeadingDirection( xAngleSupplier.getAsDouble(), yAngleSupplier.getAsDouble()); @@ -121,15 +123,15 @@ private static Rotation2d getOmegaRadiansPerSecond( return new Rotation2d(omegaSquared * maxAngularSpeedRadPerSec); } - private static Rotation2d getHeadingDirection(double xInput, double yInput) { + private static Optional getHeadingDirection(double xInput, double yInput) { // Get desired angle as a vector final Translation2d desiredAngle = new Translation2d(xInput, yInput); // If the vector length is longer then our deadband update the heading controller if (desiredAngle.getNorm() > stickDirectionDeadband.get()) { - return desiredAngle.getAngle(); + return Optional.of(desiredAngle.getAngle()); } - return null; + return Optional.empty(); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 5634b1d..ef1a7d5 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -11,7 +11,5 @@ public record CameraConfig(String cameraName, Transform3d robotToCamera) {} public static final CameraConfig FRONT_CAMERA = new CameraConfig( - "frontCam", - new Transform3d( - new Translation3d(0.805, 0, 0), new Rotation3d(0, Math.toRadians(-30), 0))); + "frontCam", new Transform3d(new Translation3d(0, 0, 0.1), new Rotation3d(0, 0, 0))); } diff --git a/src/main/java/frc/robot/utility/SpeedController.java b/src/main/java/frc/robot/utility/SpeedController.java index 77363ad..06ff83a 100644 --- a/src/main/java/frc/robot/utility/SpeedController.java +++ b/src/main/java/frc/robot/utility/SpeedController.java @@ -47,7 +47,7 @@ public ChassisSpeeds applyTo(ChassisSpeeds speeds) { /** Enum representing different speed levels with translational and rotational coefficients. */ public enum SpeedLevel { PRECISE(0.25, 0.1), - DEFAULT(0.90, 0.60), + DEFAULT(0.75, 0.60), BOOST(1.0, 0.75), MAX_BOOST(1.0, 1.0);