Merge branch 'main' of https://github.com/redshiftrobotics/preseason2024

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
   }
 }

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)));
 

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 ---

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),

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();

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();
   }

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<Rotation2d> 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<Rotation2d> 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();
   }
 }

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)));
 }

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);