Misc Fixes

Author: srimanachanta

src/main/java/frc/robot/Robot.java

@@ -35,7 +35,7 @@
  */
 public class Robot extends LoggedRobot {
   private Command autonomousCommand;
-  private RobotContainer robotContainer;
+  private final RobotContainer robotContainer;
 
   public Robot() {
     super(Constants.kLoopPeriodSecs);
@@ -73,6 +73,9 @@ public Robot() {
 
     // Configure brownout voltage
     RobotController.setBrownoutVoltage(6.0);
+
+    // Create RobotConatiner
+    robotContainer = new RobotContainer();
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -11,6 +11,9 @@
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 public class RobotContainer {
+  // Load RobotState class
+  private final RobotState robotState = RobotState.getInstance();
+
   // Subsystems
   private final DriveBase driveBase;
 

src/main/java/frc/robot/RobotState.java

@@ -25,7 +25,9 @@ public class RobotState {
   private static RobotState instance;
 
   public static RobotState getInstance() {
-    if (instance == null) instance = new RobotState();
+    if (instance == null) {
+      instance = new RobotState();
+    }
     return instance;
   }
 

src/main/java/frc/robot/commands/DriveCommands.java

@@ -32,7 +32,7 @@ public class DriveCommands {
 
   // Characterization
   private static final double FF_START_DELAY = 2.0; // Secs
-  private static final double FF_RAMP_RATE = 0.1; // Volts/Sec
+  private static final double FF_RAMP_RATE = 0.85; // Volts/Sec
   private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec
   private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2
 
@@ -184,8 +184,8 @@ public static Command feedforwardCharacterization(DriveBase drive) {
                   double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX);
 
                   NumberFormat formatter = new DecimalFormat("#0.00000");
-                  SmartDashboard.getString("kS", formatter.format(kS));
-                  SmartDashboard.getString("kV", formatter.format(kV));
+                  SmartDashboard.putString("kS", formatter.format(kS));
+                  SmartDashboard.putString("kV", formatter.format(kV));
                 }));
   }
 
@@ -245,11 +245,11 @@ public static Command wheelRadiusCharacterization(DriveBase drive) {
 
                       NumberFormat formatter = new DecimalFormat("#0.000");
 
-                      SmartDashboard.getString(
+                      SmartDashboard.putString(
                           "Wheel Delta", formatter.format(wheelDelta) + " radians");
-                      SmartDashboard.getString(
+                      SmartDashboard.putString(
                           "Gyro Delta", formatter.format(state.gyroDelta) + " radians");
-                      SmartDashboard.getString(
+                      SmartDashboard.putString(
                           "Wheel Radius",
                           formatter.format(wheelRadius)
                               + " meters, "

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -3,11 +3,13 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
 import frc.robot.util.swerve.SwerveSetpointGenerator.ModuleLimits;
 import lombok.Builder;
 
 public class DriveConstants {
-  public static final double odometryFrequencyHz = 250;
+  public static final double odometryFrequencyHz =
+      Constants.getRobotMode() == Constants.RobotMode.SIM ? 50 : 250;
 
   public static final double trackWidthX = Units.inchesToMeters(20.75);
   public static final double trackWidthY = Units.inchesToMeters(20.75);

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -34,12 +34,12 @@ public class Module {
         turnkD.initDefault(50.0);
       }
       default -> {
-        drivekS.initDefault(0.0); // TODO
-        drivekV.initDefault(0.0); // TODO
-        drivekP.initDefault(0.0); // TODO
-        drivekD.initDefault(0.0); // TODO
-        turnkP.initDefault(0.0); // TODO
-        turnkD.initDefault(0.0); // TODO
+        drivekS.initDefault(0.11400);
+        drivekV.initDefault(0.84144);
+        drivekP.initDefault(0.1);
+        drivekD.initDefault(0.0);
+        turnkP.initDefault(10.0);
+        turnkD.initDefault(0.0);
       }
     }
   }

src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import frc.robot.Constants;
+import java.util.Queue;
 
 public class ModuleIOSim implements ModuleIO {
   private static final DCMotor driveMotorModel = DCMotor.getNEO(1);
@@ -34,9 +35,18 @@ public class ModuleIOSim implements ModuleIO {
   private double driveFFVolts = 0.0;
   private double turnAppliedVolts = 0.0;
 
+  // Queue inputs from odometry thread
+  private final Queue<Double> drivePositionQueue;
+  private final Queue<Double> turnPositionQueue;
+
   public ModuleIOSim() {
     // Enable wrapping for turn PID
     turnController.enableContinuousInput(-Math.PI, Math.PI);
+
+    drivePositionQueue =
+        OdometryManager.getInstance().registerSignal(driveSim::getAngularPositionRad);
+    turnPositionQueue =
+        OdometryManager.getInstance().registerSignal(turnSim::getAngularPositionRad);
   }
 
   @Override
@@ -75,8 +85,12 @@ public void updateInputs(ModuleIOInputs inputs) {
     inputs.turnAppliedVolts = turnAppliedVolts;
     inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps());
 
-    inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad};
-    inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition};
+    inputs.odometryDrivePositionsRad =
+        drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray();
+    drivePositionQueue.clear();
+    inputs.odometryTurnPositions =
+        turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new);
+    turnPositionQueue.clear();
   }
 
   @Override

src/main/java/frc/robot/subsystems/drive/OdometryManager.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.drive;
 
 import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Queue;
@@ -49,6 +50,11 @@ public Queue<Double> registerSignal(DoubleSupplier signal) {
   private void run() {
     odometryLock.lock();
     try {
+      // Get sample timestamp
+      double timestamp = RobotController.getFPGATime() / 1e6;
+      timestampQueue.add(timestamp);
+
+      // Read signals and provide them to queues
       for (int i = 0; i < signalSuppliers.size(); i++) {
         signalQueues.get(i).add(signalSuppliers.get(i).getAsDouble());
       }