swerve logging

build.gradle

@@ -83,7 +83,7 @@ dependencies {
     testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2'
     testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2'
 
-    annotationProcessor project(":checker")
+    // annotationProcessor project(":checker")
     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
 }

src/main/java/frc/lib/util/swerve/SwerveModule.java

@@ -1,58 +1,51 @@
 package frc.lib.util.swerve;
 
+import org.littletonrobotics.junction.Logger;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.DemandType;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import frc.lib.math.Conversions;
 import frc.lib.util.ctre.CTREModuleState;
 import frc.robot.Constants;
-import frc.robot.Robot;
 
 /**
  * Base Swerve Module Class. Creates an instance of the swerve module
  */
 public class SwerveModule {
     public int moduleNumber;
     private double angleOffset;
-    private TalonFX angleMotor;
-    private TalonFX driveMotor;
-    private CANCoder angleEncoder;
     private double lastAngle;
     SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.DRIVE_KS,
         Constants.Swerve.DRIVE_KV, Constants.Swerve.DRIVE_KA);
 
+    private SwerveModuleIO io;
+    private SwerveModuleInputsAutoLogged inputs = new SwerveModuleInputsAutoLogged();
+
     /**
      * Creates an instance of a Swerve Module
      *
      * @param moduleNumber Swerve Module ID. Must be unique
      * @param constants Constants specific to the swerve module
      */
-    public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants constants) {
+    public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants constants,
+        SwerveModuleIO io) {
+        this.io = io;
+        io.updateInputs(inputs);
+
         this.moduleNumber = moduleNumber;
         angleOffset = constants.angleOffset;
 
-        /* Angle Encoder Config */
-        angleEncoder = new CANCoder(constants.cancoderID, "canivore");
-        configAngleEncoder();
-
-        /* Angle Motor Config */
-        angleMotor = new TalonFX(constants.angleMotorID, "canivore");
-        configAngleMotor();
-
-        /* Drive Motor Config */
-        driveMotor = new TalonFX(constants.driveMotorID, "canivore");
-        configDriveMotor();
-
-
-
         lastAngle = getState().angle.getDegrees();
     }
 
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.getInstance().processInputs("SwerveModule" + moduleNumber, inputs);
+    }
+
     /**
      * Sets the desired state for the swerve module for speed and angle
      *
@@ -72,11 +65,12 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop)
 
         if (isOpenLoop) {
             double percentOutput = desiredState.speedMetersPerSecond / Constants.Swerve.MAX_SPEED;
-            driveMotor.set(ControlMode.PercentOutput, percentOutput);
+            io.setDriveMotor(ControlMode.PercentOutput, percentOutput);
         } else {
             double velocity = Conversions.mpsToFalcon(desiredState.speedMetersPerSecond,
                 Constants.Swerve.WHEEL_CIRCUMFERENCE, Constants.Swerve.DRIVE_GEAR_RATIO);
-            driveMotor.set(ControlMode.Velocity, velocity, DemandType.ArbitraryFeedForward,
+
+            io.setDriveMotorWithFF(ControlMode.Velocity, velocity, DemandType.ArbitraryFeedForward,
                 feedforward.calculate(desiredState.speedMetersPerSecond));
         }
 
@@ -86,20 +80,11 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop)
                 : desiredState.angle.getDegrees(); // Prevent rotating module if speed is
         // less then 1%. Prevents
         // Jittering.
-        angleMotor.set(ControlMode.Position,
+        io.setAngleMotor(ControlMode.Position,
             Conversions.degreesToFalcon(angle, Constants.Swerve.ANGLE_GEAR_RATIO));
         lastAngle = angle;
     }
 
-    /**
-     *
-     */
-    private void resetToAbsolute() {
-        double absolutePosition = Conversions.degreesToFalcon(
-            getCanCoder().getDegrees() - angleOffset, Constants.Swerve.ANGLE_GEAR_RATIO);
-        angleMotor.setSelectedSensorPosition(absolutePosition);
-    }
-
     /**
      *
      *
@@ -108,8 +93,8 @@ private void resetToAbsolute() {
     public void setTurnAngle(double rotationSpeed) {
         double absolutePosition = Conversions.degreesToFalcon(
             getCanCoder().getDegrees() - angleOffset, Constants.Swerve.ANGLE_GEAR_RATIO);
-        angleMotor.setSelectedSensorPosition(absolutePosition);
-        driveMotor.set(ControlMode.PercentOutput, rotationSpeed);
+        io.setAngleSelectedSensorPosition(absolutePosition);
+        io.setDriveMotor(ControlMode.PercentOutput, rotationSpeed);
     }
 
     /**
@@ -118,51 +103,21 @@ public void setTurnAngle(double rotationSpeed) {
      * @return Swerve module position
      */
     public SwerveModulePosition getPosition() {
-        double position = Conversions.falconToMeters(driveMotor.getSelectedSensorPosition(),
+        double position = Conversions.falconToMeters(inputs.driveMotorSelectedPosition,
             Constants.Swerve.DRIVE_GEAR_RATIO, Constants.Swerve.WHEEL_CIRCUMFERENCE);
 
-        Rotation2d angle = Rotation2d.fromDegrees(Conversions.falconToDegrees(
-            angleMotor.getSelectedSensorPosition(), Constants.Swerve.ANGLE_GEAR_RATIO));
+        Rotation2d angle = Rotation2d.fromDegrees(Conversions
+            .falconToDegrees(inputs.angleMotorSelectedPosition, Constants.Swerve.ANGLE_GEAR_RATIO));
         return new SwerveModulePosition(position, angle);
     }
 
-    /**
-     * Configure the Angle motor CANCoder
-     */
-    private void configAngleEncoder() {
-        angleEncoder.configFactoryDefault();
-        angleEncoder.configAllSettings(Robot.ctreConfigs.swerveCanCoderConfig);
-    }
-
-    /**
-     * Configure the Angle motor
-     */
-    private void configAngleMotor() {
-        angleMotor.configFactoryDefault();
-        angleMotor.configAllSettings(Robot.ctreConfigs.swerveAngleFXConfig);
-        angleMotor.setInverted(Constants.Swerve.ANGLE_MOTOT_INVERT);
-        angleMotor.setNeutralMode(Constants.Swerve.ANGLE_NEUTRAL_MODE);
-        resetToAbsolute();
-    }
-
-    /**
-     * Configure the Drive motor
-     */
-    private void configDriveMotor() {
-        driveMotor.configFactoryDefault();
-        driveMotor.configAllSettings(Robot.ctreConfigs.swerveDriveFXConfig);
-        driveMotor.setInverted(Constants.Swerve.DRIVE_MOTOR_INVERT);
-        driveMotor.setNeutralMode(Constants.Swerve.DRIVE_NEUTRAL_MODE);
-        driveMotor.setSelectedSensorPosition(0);
-    }
-
     /**
      * Get the 2d rotation of the module
      *
      * @return 2d rotation of the module
      */
     public Rotation2d getCanCoder() {
-        return Rotation2d.fromDegrees(angleEncoder.getAbsolutePosition());
+        return Rotation2d.fromDegrees(inputs.angleEncoderValue);
     }
 
     /**
@@ -171,10 +126,10 @@ public Rotation2d getCanCoder() {
      * @return Swerve module state
      */
     public SwerveModuleState getState() {
-        double velocity = Conversions.falconToMPS(driveMotor.getSelectedSensorVelocity(),
+        double velocity = Conversions.falconToMPS(inputs.driveMotorSelectedSensorVelocity,
             Constants.Swerve.WHEEL_CIRCUMFERENCE, Constants.Swerve.DRIVE_GEAR_RATIO);
-        Rotation2d angle = Rotation2d.fromDegrees(Conversions.falconToDegrees(
-            angleMotor.getSelectedSensorPosition(), Constants.Swerve.ANGLE_GEAR_RATIO));
+        Rotation2d angle = Rotation2d.fromDegrees(Conversions
+            .falconToDegrees(inputs.angleMotorSelectedPosition, Constants.Swerve.ANGLE_GEAR_RATIO));
         return new SwerveModuleState(velocity, angle);
     }
 

src/main/java/frc/lib/util/swerve/SwerveModuleIO.java

@@ -0,0 +1,30 @@
+package frc.lib.util.swerve;
+
+import org.littletonrobotics.junction.AutoLog;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.DemandType;
+
+public interface SwerveModuleIO {
+
+    @AutoLog
+    public static class SwerveModuleInputs {
+        public double angleEncoderValue;
+        public double driveMotorSelectedPosition;
+        public double driveMotorSelectedSensorVelocity;
+        public double angleMotorSelectedPosition;
+    }
+
+    public default void updateInputs(SwerveModuleInputs inputs) {}
+
+    public default void setDriveMotor(ControlMode mode, double outputValue) {
+
+    }
+
+    public default void setDriveMotorWithFF(ControlMode mode, double demand0,
+        DemandType demand1Type, double demand1) {}
+
+    public default void setAngleMotor(ControlMode mode, double outputValue) {}
+
+    public default void setAngleSelectedSensorPosition(double angle) {}
+
+}

src/main/java/frc/lib/util/swerve/SwerveModuleReal.java

@@ -0,0 +1,105 @@
+package frc.lib.util.swerve;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.DemandType;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.lib.math.Conversions;
+import frc.robot.Constants;
+import frc.robot.Robot;
+
+public class SwerveModuleReal implements SwerveModuleIO {
+
+    private TalonFX angleMotor; // Output
+    private TalonFX driveMotor; // Output
+    private CANCoder angleEncoder; // Input
+
+    private SwerveModuleConstants constants;
+
+    public SwerveModuleReal(SwerveModuleConstants constants) {
+        this.constants = constants;
+
+        /* Angle Encoder Config */
+        angleEncoder = new CANCoder(constants.cancoderID, "canivore");
+        configAngleEncoder();
+
+        /* Angle Motor Config */
+        angleMotor = new TalonFX(constants.angleMotorID, "canivore");
+        configAngleMotor();
+
+        /* Drive Motor Config */
+        driveMotor = new TalonFX(constants.driveMotorID, "canivore");
+        configDriveMotor();
+    }
+
+    /**
+     *
+     */
+    private void resetToAbsolute() {
+        double absolutePosition = Conversions
+            .degreesToFalcon(Rotation2d.fromDegrees(angleEncoder.getAbsolutePosition()).getDegrees()
+                - constants.angleOffset, Constants.Swerve.ANGLE_GEAR_RATIO);
+        angleMotor.setSelectedSensorPosition(absolutePosition);
+    }
+
+    /**
+     * Configure the Angle motor CANCoder
+     */
+    private void configAngleEncoder() {
+        angleEncoder.configFactoryDefault();
+        angleEncoder.configAllSettings(Robot.ctreConfigs.swerveCanCoderConfig);
+    }
+
+    /**
+     * Configure the Angle motor
+     */
+    private void configAngleMotor() {
+        angleMotor.configFactoryDefault();
+        angleMotor.configAllSettings(Robot.ctreConfigs.swerveAngleFXConfig);
+        angleMotor.setInverted(Constants.Swerve.ANGLE_MOTOT_INVERT);
+        angleMotor.setNeutralMode(Constants.Swerve.ANGLE_NEUTRAL_MODE);
+        resetToAbsolute();
+    }
+
+    /**
+     * Configure the Drive motor
+     */
+    private void configDriveMotor() {
+        driveMotor.configFactoryDefault();
+        driveMotor.configAllSettings(Robot.ctreConfigs.swerveDriveFXConfig);
+        driveMotor.setInverted(Constants.Swerve.DRIVE_MOTOR_INVERT);
+        driveMotor.setNeutralMode(Constants.Swerve.DRIVE_NEUTRAL_MODE);
+        driveMotor.setSelectedSensorPosition(0);
+    }
+
+    @Override
+    public void updateInputs(SwerveModuleInputs inputs) {
+        inputs.angleEncoderValue = angleEncoder.getAbsolutePosition();
+        inputs.angleMotorSelectedPosition = angleMotor.getSelectedSensorPosition();
+        inputs.driveMotorSelectedPosition = driveMotor.getSelectedSensorPosition();
+        inputs.driveMotorSelectedSensorVelocity = driveMotor.getSelectedSensorVelocity();
+    }
+
+    @Override
+    public void setAngleMotor(ControlMode mode, double outputValue) {
+        angleMotor.set(mode, outputValue);
+    }
+
+    @Override
+    public void setAngleSelectedSensorPosition(double angle) {
+        angleMotor.setSelectedSensorPosition(angle);
+    }
+
+    @Override
+    public void setDriveMotor(ControlMode mode, double outputValue) {
+        driveMotor.set(mode, outputValue);
+    }
+
+    @Override
+    public void setDriveMotorWithFF(ControlMode mode, double demand0, DemandType demand1Type,
+        double demand1) {
+        driveMotor.set(mode, demand0, demand1Type, demand1);
+    }
+
+}

src/main/java/frc/robot/Constants.java

@@ -181,7 +181,8 @@ public static final class Mod3 {
                 DRIVE_MOTOR_ID, ANGLE_MOTOR_ID, CAN_CODER_ID, ANGLE_OFFSET);
         }
 
-
+        public static final SwerveModuleConstants[] SWERVE_CONSTANTS =
+            {Mod0.constants, Mod1.constants, Mod2.constants, Mod3.constants};
 
     }
 

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

@@ -4,9 +4,16 @@
 
 package frc.robot;
 
+import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.lib.util.Scoring;
@@ -37,6 +44,20 @@ public class Robot extends LoggedRobot {
      */
     @Override
     public void robotInit() {
+        Logger logger = Logger.getInstance();
+        if (isReal()) {
+            logger.addDataReceiver(new WPILOGWriter("/media/sda1"));
+            logger.addDataReceiver(new NT4Publisher());
+            new PowerDistribution(1, ModuleType.kRev);
+        } else {
+            String path = LogFileUtil.findReplayLog();
+            logger.setReplaySource(new WPILOGReader(path));
+            logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(path, "_sim")));
+            setUseTiming(false);
+        }
+
+        logger.start();
+
         ctreConfigs = new CTREConfigs();
         // Instantiate our RobotContainer. This will perform all our button bindings, and put our
         // autonomous chooser on the dashboard.