Merge pull request #26 from CrossTheRoadElec/swerve-sim-update

Update CommandSwerveDrivetrain to use latest generated sim code

java/CANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java

@@ -24,19 +24,18 @@ class TalonFXSimProfile extends SimProfile {
      *
      * @param falcon
      *                        The TalonFX device
-     * @param accelToFullTime
-     *                        The time the motor takes to accelerate from 0 to full,
-     *                        in seconds
-     * @param fullVel
-     *                        The maximum motor velocity, in rotations per second
-     * @param sensorPhase
-     *                        The phase of the TalonFX sensors
+     * @param canCoder
+     *                        The CANcoder associated with the TalonFX
+     * @param gearRatio
+     *                        The gear ratio from the TalonFX to the mechanism
+     * @param rotorInertia
+     *                        Rotational Inertia of the mechanism at the rotor
      */
     public TalonFXSimProfile(final TalonFX falcon, final CANcoder canCoder, final double gearRatio, final double rotorInertia) {
         this._falcon = falcon;
         this._canCoder = canCoder;
         this._gearRatio = gearRatio;
-        this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), gearRatio, .001);
+        this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), gearRatio, rotorInertia);
     }
 
     /**

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

@@ -5,12 +5,11 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.XboxController;
 import frc.robot.commands.DriveStraightCommand;
 import frc.robot.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -25,7 +24,7 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
 
-  private final XboxController m_joystick = new XboxController(0);
+  private final CommandXboxController m_joystick = new CommandXboxController(0);
 
   /* invert the joystick Y because forward Y is negative */
   private final Command m_teleopDrive = new RunCommand(() -> {
@@ -55,7 +54,7 @@ public RobotContainer() {
    */
   private void configureButtonBindings() {
     /* If the upper left shoulder button is pressed, drive straight */
-    new Trigger(m_joystick::getLeftBumper).whileTrue(m_driveStraight);
+    m_joystick.leftBumper().whileTrue(m_driveStraight);
   }
 
   /**

java/FusedCANcoder/src/main/java/frc/robot/sim/TalonFXSimProfile.java

@@ -24,13 +24,12 @@ class TalonFXSimProfile extends SimProfile {
      * 
      * @param falcon
      *                        The TalonFX device
-     * @param accelToFullTime
-     *                        The time the motor takes to accelerate from 0 to full,
-     *                        in seconds
-     * @param fullVel
-     *                        The maximum motor velocity, in rotations per second
-     * @param sensorPhase
-     *                        The phase of the TalonFX sensors
+     * @param canCoder
+     *                        The CANcoder associated with the TalonFX
+     * @param gearRatio
+     *                        The gear ratio from the TalonFX to the mechanism
+     * @param rotorInertia
+     *                        Rotational Inertia of the mechanism at the rotor
      */
     public TalonFXSimProfile(final TalonFX falcon, final CANcoder canCoder, final double gearRatio, final double rotorInertia) {
         this._falcon = falcon;

java/MotionMagic/src/main/java/frc/robot/sim/TalonFXSimProfile.java

@@ -21,13 +21,8 @@ class TalonFXSimProfile extends SimProfile {
      * 
      * @param falcon
      *                        The TalonFX device
-     * @param accelToFullTime
-     *                        The time the motor takes to accelerate from 0 to full,
-     *                        in seconds
-     * @param fullVel
-     *                        The maximum motor velocity, in rotations per second
-     * @param sensorPhase
-     *                        The phase of the TalonFX sensors
+     * @param rotorInertia
+     *                        Rotational Inertia of the mechanism at the rotor
      */
     public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) {
         this._falcon = falcon;

java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java

@@ -23,13 +23,10 @@ class TalonFXSimProfile extends SimProfile {
      * 
      * @param falcon
      *                        The TalonFX device
-     * @param accelToFullTime
-     *                        The time the motor takes to accelerate from 0 to full,
-     *                        in seconds
-     * @param fullVel
-     *                        The maximum motor velocity, in rotations per second
-     * @param sensorPhase
-     *                        The phase of the TalonFX sensors
+     * @param pigeon
+     *                        The Pigeon 2 associated with the TalonFX
+     * @param rotorInertia
+     *                        Rotational Inertia of the mechanism at the rotor
      */
     public TalonFXSimProfile(final TalonFX falcon, final Pigeon2 pigeon, final double rotorInertia) {
         this._falcon = falcon;

java/PositionClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java

@@ -22,13 +22,8 @@ class TalonFXSimProfile extends SimProfile {
      * 
      * @param falcon
      *                        The TalonFX device
-     * @param accelToFullTime
-     *                        The time the motor takes to accelerate from 0 to full,
-     *                        in seconds
-     * @param fullVel
-     *                        The maximum motor velocity, in rotations per second
-     * @param sensorPhase
-     *                        The phase of the TalonFX sensors
+     * @param rotorInertia
+     *                        Rotational Inertia of the mechanism at the rotor
      */
     public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) {
         this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, rotorInertia);

java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java

@@ -2,6 +2,7 @@
 
 import java.util.function.Supplier;
 
+import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
@@ -13,6 +14,7 @@
 import com.pathplanner.lib.util.ReplanningConfig;
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
@@ -22,15 +24,25 @@
  * so it can be used in command-based projects easily.
  */
 public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem {
+    private static final double kSimLoopPeriod = 0.005; // 5 ms
+    private Notifier m_simNotifier = null;
+    private double m_lastSimTime;
+
     private final SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds();
 
     public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) {
         super(driveTrainConstants, OdometryUpdateFrequency, modules);
         configurePathPlanner();
+        if (Utils.isSimulation()) {
+            startSimThread();
+        }
     }
     public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) {
         super(driveTrainConstants, modules);
         configurePathPlanner();
+        if (Utils.isSimulation()) {
+            startSimThread();
+        }
     }
 
     private void configurePathPlanner() {
@@ -56,13 +68,22 @@ public Command getAutoPath(String pathName) {
         return new PathPlannerAuto(pathName);
     }
 
-    @Override
-    public void simulationPeriodic() {
-        /* Assume 20ms update rate, get battery voltage from WPILib */
-        updateSimState(0.02, RobotController.getBatteryVoltage());
-    }
-
     public ChassisSpeeds getCurrentRobotChassisSpeeds() {
         return m_kinematics.toChassisSpeeds(getState().ModuleStates);
     }
+
+    private void startSimThread() {
+        m_lastSimTime = Utils.getCurrentTimeSeconds();
+
+        /* Run simulation at a faster rate so PID gains behave more reasonably */
+        m_simNotifier = new Notifier(() -> {
+            final double currentTime = Utils.getCurrentTimeSeconds();
+            double deltaTime = currentTime - m_lastSimTime;
+            m_lastSimTime = currentTime;
+
+            /* use the measured time delta, get battery voltage from WPILib */
+            updateSimState(deltaTime, RobotController.getBatteryVoltage());
+        });
+        m_simNotifier.startPeriodic(kSimLoopPeriod);
+    }
 }

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

@@ -4,9 +4,6 @@
 
 package frc.robot;
 
-
-import com.ctre.phoenix6.SignalLogger;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
@@ -25,8 +22,6 @@ public void robotInit() {
     m_robotContainer = new RobotContainer();
 
     m_robotContainer.drivetrain.getDaqThread().setThreadPriority(99);
-
-    SignalLogger.start();
   }
   @Override
   public void robotPeriodic() {

java/SwerveWithPathPlanner/src/main/java/frc/robot/Telemetry.java

@@ -1,5 +1,6 @@
 package frc.robot;
 
+import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState;
 
@@ -10,6 +11,8 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.util.datalog.DoubleArrayLogEntry;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
@@ -20,8 +23,8 @@
 
 public class Telemetry {
     private final double MaxSpeed;
-    private int logEntry;
-    private int odomEntry;
+    private final DoubleArrayLogEntry logEntry;
+    private final DoubleLogEntry odomEntry;
 
     /**
      * Construct a telemetry object, with the specified max speed of the robot
@@ -30,53 +33,54 @@ public class Telemetry {
      */
     public Telemetry(double maxSpeed) {
         MaxSpeed = maxSpeed;
-        logEntry = DataLogManager.getLog().start("odometry", "double[]");
-        odomEntry = DataLogManager.getLog().start("odom period", "double");
+        SignalLogger.start();
+        logEntry = new DoubleArrayLogEntry(DataLogManager.getLog(), "odometry");
+        odomEntry = new DoubleLogEntry(DataLogManager.getLog(), "odom period");
     }
 
     /* What to publish over networktables for telemetry */
-    NetworkTableInstance inst = NetworkTableInstance.getDefault();
+    private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
     /* Robot pose for field positioning */
-    NetworkTable table = inst.getTable("Pose");
-    DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
-    StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
+    private final NetworkTable table = inst.getTable("Pose");
+    private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
+    private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
 
     /* Robot speeds for general checking */
-    NetworkTable driveStats = inst.getTable("Drive");
-    DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
-    DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
-    DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
-    DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish();
+    private final NetworkTable driveStats = inst.getTable("Drive");
+    private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
+    private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
+    private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
+    private final DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish();
 
     /* Keep a reference of the last pose to calculate the speeds */
-    Pose2d m_lastPose = new Pose2d();
-    double lastTime = Utils.getCurrentTimeSeconds();
+    private Pose2d m_lastPose = new Pose2d();
+    private double lastTime = Utils.getCurrentTimeSeconds();
 
     /* Mechanisms to represent the swerve module states */
-    Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
-            new Mechanism2d(1, 1),
-            new Mechanism2d(1, 1),
-            new Mechanism2d(1, 1),
-            new Mechanism2d(1, 1),
+    private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
+        new Mechanism2d(1, 1),
+        new Mechanism2d(1, 1),
+        new Mechanism2d(1, 1),
+        new Mechanism2d(1, 1),
     };
     /* A direction and length changing ligament for speed representation */
-    MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] {
-            m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
-            m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
-            m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
-            m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+    private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] {
+        m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
     };
     /* A direction changing and length constant ligament for module direction */
-    MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] {
-            m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
-                    .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
-            m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
-                    .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
-            m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
-                    .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
-            m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
-                    .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+    private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] {
+        m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
     };
 
     /* Accept the swerve drive state and telemeterize it to smartdashboard */
@@ -85,9 +89,9 @@ public void telemeterize(SwerveDriveState state) {
         Pose2d pose = state.Pose;
         fieldTypePub.set("Field2d");
         fieldPub.set(new double[] {
-                pose.getX(),
-                pose.getY(),
-                pose.getRotation().getDegrees()
+            pose.getX(),
+            pose.getY(),
+            pose.getRotation().getDegrees()
         });
 
         /* Telemeterize the robot's general speeds */
@@ -113,7 +117,8 @@ public void telemeterize(SwerveDriveState state) {
             SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
         }
 
-        DataLogManager.getLog().appendDoubleArray(logEntry, new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}, (long)(Timer.getFPGATimestamp() * 1000000));
-        DataLogManager.getLog().appendDouble(odomEntry, state.OdometryPeriod, (long)(Timer.getFPGATimestamp() * 1000000));
+        long timestamp = (long)(Timer.getFPGATimestamp() * 1000000);
+        logEntry.append(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}, timestamp);
+        odomEntry.append(state.OdometryPeriod, timestamp);
     }
 }

java/VelocityClosedLoop/src/main/java/frc/robot/sim/TalonFXSimProfile.java

@@ -21,13 +21,8 @@ class TalonFXSimProfile extends SimProfile {
      * 
      * @param falcon
      *                        The TalonFX device
-     * @param accelToFullTime
-     *                        The time the motor takes to accelerate from 0 to full,
-     *                        in seconds
-     * @param fullVel
-     *                        The maximum motor velocity, in rotations per second
-     * @param sensorPhase
-     *                        The phase of the TalonFX sensors
+     * @param rotorInertia
+     *                        Rotational Inertia of the mechanism at the rotor
      */
     public TalonFXSimProfile(final TalonFX falcon, final double rotorInertia) {
         this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, rotorInertia);