Add motor logging

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

@@ -144,7 +144,7 @@ public static class IntakeConstants {
             / BOTTOM_GEARING;
     public static final double MAX_TOP_RPM =
         Units.radiansPerSecondToRotationsPerMinute(INTAKE_MOTOR.freeSpeedRadPerSec) / TOP_GEARING;
-    public static final double MOI = 0.1;
+    public static final double MOI = 0.00005;
     public static final int TOP_CURRENT_LIMIT = 20;
     public static final int BOTTOM_CURRENT_LIMIT = 20;
     public static final double TOP_POSITION_CONVERSION_FACTOR = 1 / TOP_GEARING;

src/main/java/frc/robot/subsystems/elevatorIO/Elevator.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -12,6 +13,7 @@
 import frc.robot.Constants;
 import frc.robot.Robot;
 import frc.robot.Robot.GamePieceMode;
+import frc.robot.util.LoggableMotor;
 import frc.robot.util.RedHawkUtil;
 import frc.robot.util.SuperstructureConfig;
 import org.littletonrobotics.junction.Logger;
@@ -24,6 +26,7 @@ public class Elevator extends SubsystemBase {
   private double targetHeight = 0.0;
   public boolean manualControl = false;
   private final ElevatorFeedforward feedforward;
+  private LoggableMotor leftMotor, rightMotor;
 
   public Elevator(ElevatorIO IO) {
     this.feedforward = Constants.ElevatorConstants.ELEVATOR_GAINS.createElevatorFeedforward();
@@ -34,6 +37,8 @@ public Elevator(ElevatorIO IO) {
     this.inputs = new ElevatorInputsAutoLogged();
     IO.updateInputs(inputs);
     this.IO = IO;
+    leftMotor = new LoggableMotor("Elevator Left", DCMotor.getNEO(1));
+    rightMotor = new LoggableMotor("Elevator Right", DCMotor.getNEO(1));
   }
 
   public void setTargetHeight(double targetHeightInches) {
@@ -70,6 +75,8 @@ public void resetController() {
 
   public void periodic() {
     IO.updateInputs(inputs);
+    leftMotor.log(inputs.currentDrawAmpsLeft, inputs.outputVoltageLeft);
+    rightMotor.log(inputs.currentDrawAmpsRight, inputs.outputVoltageRight);
 
     Logger.getInstance()
         .recordOutput("Elevator/Left is NaN", Double.isNaN(inputs.heightInchesLeft));

src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIOSim.java

@@ -37,12 +37,12 @@ public void updateInputs(ElevatorInputs inputs) {
     inputs.heightInchesLeft = Units.metersToInches(sim.getPositionMeters());
     inputs.velocityInchesPerSecondLeft = Units.metersToInches(sim.getVelocityMetersPerSecond());
     inputs.tempCelsiusLeft = 0.0;
-    inputs.currentDrawAmpsLeft = sim.getCurrentDrawAmps();
+    inputs.currentDrawAmpsLeft = sim.getCurrentDrawAmps() / 2.0;
     inputs.outputVoltageRight = MathUtil.clamp(sim.getOutput(0), -12.0, 12.0);
     inputs.heightInchesRight = Units.metersToInches(sim.getPositionMeters());
     inputs.velocityInchesPerSecondRight = Units.metersToInches(sim.getVelocityMetersPerSecond());
     inputs.tempCelsiusRight = 0.0;
-    inputs.currentDrawAmpsRight = sim.getCurrentDrawAmps();
+    inputs.currentDrawAmpsRight = sim.getCurrentDrawAmps() / 2.0;
   }
 
   public boolean shouldApplyFF() {

src/main/java/frc/robot/subsystems/fourBarIO/FourBar.java

@@ -6,6 +6,7 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
@@ -18,6 +19,7 @@
 import frc.robot.Constants.FourBarConstants;
 import frc.robot.Robot;
 import frc.robot.Robot.GamePieceMode;
+import frc.robot.util.LoggableMotor;
 import frc.robot.util.RedHawkUtil;
 import frc.robot.util.SuperstructureConfig;
 import org.littletonrobotics.junction.Logger;
@@ -40,6 +42,7 @@ public enum FourBarMode {
   private final ArmFeedforward ff;
   private Timer timer = new Timer();
   private double absOffset = 0;
+  private LoggableMotor motor = new LoggableMotor("FourBar", DCMotor.getNEO(1));
 
   public FourBar(FourBarIO IO) {
     this.ff = Constants.FourBarConstants.FOUR_BAR_VOLTAGE_GAINS.createArmFeedforward();
@@ -122,6 +125,7 @@ public boolean isHomed(boolean useEncoders) {
   public void periodic() {
     IO.updateInputs(inputs);
     Logger.getInstance().processInputs("4Bar", inputs);
+    motor.log(inputs.currentDrawOne, inputs.outputVoltage);
 
     double voltage = 0;
     switch (mode) {

src/main/java/frc/robot/subsystems/intakeIO/Intake.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
@@ -13,6 +14,7 @@
 import frc.robot.Robot;
 import frc.robot.Robot.GamePieceMode;
 import frc.robot.subsystems.LightStrip.Pattern;
+import frc.robot.util.LoggableMotor;
 import org.littletonrobotics.junction.Logger;
 
 public class Intake extends SubsystemBase {
@@ -30,11 +32,14 @@ public class Intake extends SubsystemBase {
   private Debouncer debouncer = new Debouncer(0.1);
   private LinearFilter analogVoltageFilterRight = LinearFilter.singlePoleIIR(0.04, 0.02);
   private LinearFilter analogVoltageFilterLeft = LinearFilter.singlePoleIIR(0.04, 0.02);
+  private LoggableMotor topRollerMotor, bottomRollerMotor;
 
   public Intake(IntakeIO IO) {
     this.inputs = new IntakeInputsAutoLogged();
     IO.updateInputs(inputs);
     this.IO = IO;
+    topRollerMotor = new LoggableMotor("Intake Top Roller", DCMotor.getNeo550(1));
+    bottomRollerMotor = new LoggableMotor("Intake Bottom Roller", DCMotor.getNeo550(1));
   }
 
   public boolean isAtTarget() {
@@ -70,8 +75,9 @@ public void setScoring(boolean scoring) {
   }
 
   public void periodic() {
-
     IO.updateInputs(inputs);
+    topRollerMotor.log(inputs.topCurrentAmps, inputs.topOutputVoltage);
+    bottomRollerMotor.log(inputs.bottomCurrentAmps, inputs.bottomOutputVoltage);
 
     filteredVoltageCube = analogVoltageFilterRight.calculate(inputs.encoderVoltageRight);
     filteredVoltageCone = analogVoltageFilterLeft.calculate(inputs.encoderVoltageLeft);

src/main/java/frc/robot/subsystems/swerveIO/module/SwerveModule.java

@@ -3,8 +3,10 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Robot;
+import frc.robot.util.LoggableMotor;
 import frc.robot.util.PIDFFController;
 import org.littletonrobotics.junction.Logger;
 
@@ -19,6 +21,8 @@ public class SwerveModule extends SubsystemBase {
   SwerveModuleState state;
   public final ModuleInfo information;
 
+  LoggableMotor azimuthMotor, driveMotor;
+
   /**
    * Creates a new SwerveModule object.
    *
@@ -36,6 +40,10 @@ public SwerveModule(SwerveModuleIO swerveModuleIO, ModuleInfo information) {
 
     state = new SwerveModuleState(0, Rotation2d.fromDegrees(inputs.aziEncoderPositionDeg));
     azimuthController.enableContinuousInput(-180, 180);
+
+    azimuthMotor =
+        new LoggableMotor("Swerve " + information.getName() + " Azimuth", DCMotor.getNEO(1));
+    driveMotor = new LoggableMotor("Swerve " + information.getName() + " Drive", DCMotor.getNEO(1));
   }
 
   /**
@@ -133,9 +141,11 @@ public void update() {
 
   @Override
   public void periodic() {
+    io.updateInputs(inputs);
+    azimuthMotor.log(inputs.aziCurrentDrawAmps, inputs.aziOutputVolts);
+    driveMotor.log(inputs.driveCurrentDrawAmps, inputs.driveOutputVolts);
     update();
 
-    io.updateInputs(inputs);
     Logger.getInstance().processInputs("Swerve/" + information.getName(), inputs);
     recordOutput("Azimuth Error", state.angle.getDegrees() - inputs.aziEncoderPositionDeg);
     recordOutput(

src/main/java/frc/robot/util/LoggableMotor.java

@@ -0,0 +1,27 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import org.littletonrobotics.junction.Logger;
+
+public class LoggableMotor {
+  private String name;
+  private DCMotor motor;
+
+  public LoggableMotor(String name, DCMotor motor) {
+    this.name = name;
+    this.motor = motor;
+  }
+
+  public void log(double currentDrawAmps, double volts) {
+    String name = String.format("Motors/%s", this.name);
+
+    double torque = motor.getTorque(currentDrawAmps);
+    double velocity = motor.getSpeed(torque, volts);
+
+    Logger.getInstance().recordOutput(String.format("%s/CurrentAmps", name), currentDrawAmps);
+    Logger.getInstance().recordOutput(String.format("%s/VelocityRPM", name), velocity);
+    Logger.getInstance().recordOutput(String.format("%s/Volts", name), volts);
+    Logger.getInstance().recordOutput(String.format("%s/TorqueNM", name), torque);
+    Logger.getInstance().recordOutput(String.format("%s/PowerWatts", name), torque * velocity);
+  }
+}