Reduce boilerplate for advantagekit logging (#75)

* struct serialization for motor inputs * javadoc * standardize reading from CANcoders
FRC3476 · Apr 5, 2024 · c1a581e · c1a581e
1 parent 60d0a0e
commit c1a581e
Show file tree

Hide file tree

Showing 22 changed files with 233 additions and 311 deletions.
diff --git a/src/main/java/org/codeorange/frc2024/subsystem/arm/Arm.java b/src/main/java/org/codeorange/frc2024/subsystem/arm/Arm.java
@@ -34,7 +34,7 @@ public synchronized void update() {
         //position, velocity, and acceleration of the profile at that time
 
     public double getPivotDegrees() {
-        return inputs.leadRelativePosition;
+        return inputs.leadMotor.position;
     }
 
     public void resetPosition() {

diff --git a/src/main/java/org/codeorange/frc2024/subsystem/arm/ArmIO.java b/src/main/java/org/codeorange/frc2024/subsystem/arm/ArmIO.java
@@ -1,23 +1,14 @@
 package org.codeorange.frc2024.subsystem.arm;
 
+import org.codeorange.frc2024.utility.logging.MotorInputs;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ArmIO {
     @AutoLog
     class ArmInputs {
-        double leadAbsolutePosition = 0.0;
-        double leadVelocity = 0.0;
-        double leadRelativePosition = 0.0;
-        double leadAccel = 0.0;
-        double leadCurrent = 0.0;
-        double leadTemp = 0.0;
-        double leadVoltage = 0.0;
-
-        double followPosition = 0.0;
-        double followVelocity = 0.0;
-        double followCurrent = 0.0;
-        double followTemp = 0.0;
-        double followVoltage = 0.0;
+        double absolutePosition = 0.0;
+        MotorInputs leadMotor;
+        MotorInputs followMotor;
     }
 
     default void updateInputs(ArmInputs inputs) {}

diff --git a/src/main/java/org/codeorange/frc2024/subsystem/arm/ArmIOTalonFX.java b/src/main/java/org/codeorange/frc2024/subsystem/arm/ArmIOTalonFX.java
@@ -8,6 +8,7 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.*;
 import org.codeorange.frc2024.utility.OrangeUtility;
+import org.codeorange.frc2024.utility.logging.TalonFXAutoLogger;
 
 import static org.codeorange.frc2024.robot.Constants.*;
 import static org.codeorange.frc2024.robot.Constants.Ports.*;
@@ -19,26 +20,15 @@ public class ArmIOTalonFX implements ArmIO {
     private final CANcoder absoluteEncoder;
 
 
-    private final StatusSignal<Double> leadAbsolutePosition;
-    private final StatusSignal<Double> leadRelativePosition;
-    private final StatusSignal<Double> leadVelocity;
-    private final StatusSignal<Double> leadAccel;
-    private final StatusSignal<Double> leadCurrent;
-    private final StatusSignal<Double> leadTemp;
-    private final StatusSignal<Double> leadVoltage;
-    private final StatusSignal<Double> followVelocity;
-    private final StatusSignal<Double> followPosition;
-    private final StatusSignal<Double> followTemp;
-    private final StatusSignal<Double> followVoltage;
-    private final StatusSignal<Double> followCurrent;
+    private final StatusSignal<Double> absolutePosition;
+    private final TalonFXAutoLogger leadMotorLogger;
+    private final TalonFXAutoLogger followMotorLogger;
     private final MotionMagicVoltage motionMagicControl = new MotionMagicVoltage(0).withEnableFOC(true).withOverrideBrakeDurNeutral(true);
 
 
 
     public ArmIOTalonFX() {
-
         leadTalonFX = new TalonFX(ARM_LEAD, CAN_BUS);
-
         absoluteEncoder = new CANcoder(ARM_CANCODER, CAN_BUS);
 
         var talonFXConfigs = new TalonFXConfiguration();
@@ -74,7 +64,6 @@ public ArmIOTalonFX() {
 
         OrangeUtility.betterCTREConfigApply(leadTalonFX, talonFXConfigs);
 
-
         OrangeUtility.betterCTREConfigApply(absoluteEncoder, new CANcoderConfiguration()
                 .withMagnetSensor(new MagnetSensorConfigs()
                         .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)
@@ -83,14 +72,9 @@ public ArmIOTalonFX() {
                 )
         );
 
+        absolutePosition = absoluteEncoder.getAbsolutePosition();
 
-        leadRelativePosition = leadTalonFX.getPosition();
-        leadAbsolutePosition = absoluteEncoder.getAbsolutePosition();
-        leadVelocity = leadTalonFX.getVelocity();
-        leadAccel = leadTalonFX.getAcceleration();
-        leadCurrent = leadTalonFX.getSupplyCurrent();
-        leadTemp = leadTalonFX.getDeviceTemp();
-        leadVoltage = leadTalonFX.getMotorVoltage();
+        leadMotorLogger = new TalonFXAutoLogger(leadTalonFX);
 
         if(!isPrototype()) {
             followTalonFX = new TalonFX(ARM_FOLLOW, CAN_BUS);
@@ -99,26 +83,18 @@ public ArmIOTalonFX() {
             followTalonFX.setControl(new Follower(leadTalonFX.getDeviceID(), false));
             followTalonFX.setNeutralMode(NeutralModeValue.Brake);
 
-            followPosition = followTalonFX.getPosition();
-            followVelocity = followTalonFX.getVelocity();
-            followCurrent = followTalonFX.getSupplyCurrent();
-            followTemp = followTalonFX.getDeviceTemp();
-            followVoltage = followTalonFX.getMotorVoltage();
+            followMotorLogger = new TalonFXAutoLogger(followTalonFX);
         } else {
             followTalonFX = null;
-            followPosition = null;
-            followVelocity = null;
-            followCurrent = null;
-            followTemp = null;
-            followVoltage = null;
+            followMotorLogger = null;
         }
 
         var absPos = absoluteEncoder.getAbsolutePosition().getValue();
 
         if(absPos < -0.05) {
             if(absPos > -0.30) {
                 for(var i = 0; i < 10; i++) {
-                    System.out.println("ILLEGAL CANCODER READING, CHECK");
+                    System.err.println("ILLEGAL CANCODER READING, CHECK");
                 }
                 absPos = 0.0;
             } else {
@@ -136,26 +112,14 @@ public void setPosition(double position){
     }
 
     public void updateInputs(ArmInputs inputs) {
-        BaseStatusSignal.refreshAll(leadAbsolutePosition, leadRelativePosition, leadVelocity, leadAccel, leadCurrent,
-                leadTemp, leadVoltage);
+        inputs.absolutePosition = absolutePosition.refresh().getValue();
 
-        inputs.leadAbsolutePosition = leadAbsolutePosition.getValue();
-        inputs.leadRelativePosition = leadRelativePosition.getValue();
-        inputs.leadVelocity = leadVelocity.getValue();
-        inputs.leadAccel = leadAccel.getValue();
-        inputs.leadCurrent = leadCurrent.getValue();
-        inputs.leadTemp = leadTemp.getValue();
-        inputs.leadVoltage = leadVoltage.getValue();
+        inputs.leadMotor = leadMotorLogger.update();
 
         if(!isPrototype()) {
             assert followTalonFX != null;
 
-            BaseStatusSignal.refreshAll(followPosition, followVelocity, followCurrent, followTemp, followVoltage);
-            inputs.followPosition = followPosition.getValue();
-            inputs.followVelocity = followVelocity.getValue();
-            inputs.followCurrent = followCurrent.getValue();
-            inputs.followTemp = followTemp.getValue();
-            inputs.followVoltage = followVoltage.getValue();
+            inputs.followMotor = followMotorLogger.update();
         }
     }
 

diff --git a/src/main/java/org/codeorange/frc2024/subsystem/climber/Climber.java b/src/main/java/org/codeorange/frc2024/subsystem/climber/Climber.java
@@ -31,7 +31,11 @@ public Climber(ClimberIO climberIO){
     }
 
     public void setMotorPosition(double positionInRotations) {
-        climberIO.setMotorPosition(MathUtil.clamp(positionInRotations, CLIMBER_LOWER_LIMIT_ROTATIONS, CLIMBER_UPPER_LIMIT_ROTATIONS));
+        if (climberInputs.limitSwitchPushed && positionInRotations < climberInputs.climber.position) {
+            stop();
+        } else {
+            climberIO.setMotorPosition(MathUtil.clamp(positionInRotations, CLIMBER_LOWER_LIMIT_ROTATIONS, CLIMBER_UPPER_LIMIT_ROTATIONS));
+        }
     }
 
     public void update() {
@@ -60,7 +64,7 @@ public void update() {
                 if (limitSwitchPushed()) {
                     climbing = false;
                     hasClimbed = true;
-                    holdPosition = climberInputs.climberPosition + CLIMBER_SWITCH_OFFSET;
+                    holdPosition = climberInputs.climber.position + CLIMBER_SWITCH_OFFSET;
                 }
             } else {
                 climberIO.setVoltage(4);
@@ -75,7 +79,7 @@ public void update() {
     }
 
     public double getPositionInRotations() {
-        return climberInputs.climberPosition;
+        return climberInputs.climber.position;
     }
     public void zeroEncoder() {
         climberIO.setEncoderToZero();

diff --git a/src/main/java/org/codeorange/frc2024/subsystem/climber/ClimberIO.java b/src/main/java/org/codeorange/frc2024/subsystem/climber/ClimberIO.java
@@ -1,15 +1,12 @@
 package org.codeorange.frc2024.subsystem.climber;
 
+import org.codeorange.frc2024.utility.logging.MotorInputs;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ClimberIO {
     @AutoLog
     class ClimberInputs {
-        double climberPosition = 0.0;
-        double climberVelocity = 0.0;
-        double climberCurrent = 0.0;
-        double climberTemp = 0.0;
-        double climberVoltage = 0.0;
+        MotorInputs climber;
         boolean limitSwitchPushed = false;
         //String relayValue = "hi";
 

diff --git a/src/main/java/org/codeorange/frc2024/subsystem/climber/ClimberIOTalonFX.java b/src/main/java/org/codeorange/frc2024/subsystem/climber/ClimberIOTalonFX.java
@@ -14,17 +14,13 @@
 import edu.wpi.first.wpilibj.Servo;
 import org.codeorange.frc2024.robot.Constants;
 import org.codeorange.frc2024.utility.OrangeUtility;
+import org.codeorange.frc2024.utility.logging.TalonFXAutoLogger;
 
 import static org.codeorange.frc2024.robot.Constants.*;
 
 public class ClimberIOTalonFX implements ClimberIO {
-    private final StatusSignal<Double> climberPosition;
-    private final StatusSignal<Double> climberVelocity;
-    private final StatusSignal<Double> climberCurrent;
-    private final StatusSignal<Double> climberTemp;
-    private final StatusSignal<Double> climberVoltage;
-
     private final TalonFX motor;
+    private final TalonFXAutoLogger motorLogger;
     private final Servo servoLeft;
     private final Servo servoRight;
     private final DigitalInput limitSwitch;
@@ -61,29 +57,16 @@ public ClimberIOTalonFX() {
 
         OrangeUtility.betterCTREConfigApply(motor, motorConfig);
 
-        climberPosition = motor.getPosition();
-        climberVelocity = motor.getVelocity();
-        climberVoltage = motor.getMotorVoltage();
-        climberCurrent = motor.getSupplyCurrent();
-        climberTemp = motor.getDeviceTemp();
+        motorLogger = new TalonFXAutoLogger(motor);
     }
     private final PositionVoltage motionMagicRequest = new PositionVoltage(0).withEnableFOC(true).withOverrideBrakeDurNeutral(true);
     public void setMotorPosition(double targetPosition) {
-        if (!limitSwitch.get() && targetPosition < climberPosition.getValue()) {
-            stop();
-        } else {
-            motor.setControl(motionMagicRequest.withPosition(targetPosition));
-        }
+        motor.setControl(motionMagicRequest.withPosition(targetPosition));
     }
 
     public void updateInputs(ClimberInputs inputs) {
-        BaseStatusSignal.refreshAll(climberPosition, climberVelocity, climberVoltage, climberCurrent, climberTemp);
+        inputs.climber = motorLogger.update();
 
-        inputs.climberPosition = climberPosition.getValue();
-        inputs.climberVelocity = climberVelocity.getValue();
-        inputs.climberVoltage = climberVoltage.getValue();
-        inputs.climberCurrent = climberCurrent.getValue();
-        inputs.climberTemp = climberTemp.getValue();
         inputs.limitSwitchPushed = !limitSwitch.get();
         //inputs.relayValue = spikeRelay.get().getPrettyValue();
     }

diff --git a/src/main/java/org/codeorange/frc2024/subsystem/drive/Drive.java b/src/main/java/org/codeorange/frc2024/subsystem/drive/Drive.java
@@ -171,26 +171,26 @@ public SwerveModulePosition[] getModulePositions() {
         for (int i = 0; i < 4; i++) {
             swerveModulePositions[i] = new SwerveModulePosition(
                     getDrivePosition(i),
-                    Rotation2d.fromDegrees(getWheelRotation(i)));
+                    Rotation2d.fromRotations(getWheelRotation(i)));
         }
         return swerveModulePositions;
     }
 
     private double getSwerveDriveVelocity(int motorNum) {
-        return moduleInputs[motorNum].driveMotorVelocity;
+        return moduleInputs[motorNum].driveMotor.velocity;
     }
 
     @AutoLogOutput(key = "Drive/Wheel Rotations")
     public double getWheelRotation(int moduleNumber) {
         if (USE_RELATIVE_ENCODER_POSITION) {
-            return MathUtil.normalize(moduleInputs[moduleNumber].steerMotorRelativePosition, 0, 360);
+            return MathUtil.normalize(moduleInputs[moduleNumber].steerMotor.position, 0, 1);
         } else {
             return moduleInputs[moduleNumber].steerMotorAbsolutePosition;
         }
     }
 
     public double getDrivePosition(int moduleNumber) {
-        return moduleInputs[moduleNumber].driveMotorPosition;
+        return moduleInputs[moduleNumber].driveMotor.position;
     }
 
     double[] lastModuleVelocities = new double[4];
@@ -247,7 +247,7 @@ private synchronized void setSwerveModuleStates(SecondOrderModuleState[] swerveM
             Logger.recordOutput("Drive/SwerveModule " + getModuleName(i) + "/Wanted Acceleration", 0);
             Logger.recordOutput("Drive/SwerveModule " + getModuleName(i) + "/Wanted Angular Speed", moduleState.omega);
 
-            realStates[i] = new SwerveModuleState(moduleInputs[i].driveMotorVelocity, Rotation2d.fromDegrees(moduleInputs[i].steerMotorRelativePosition));
+            realStates[i] = new SwerveModuleState(moduleInputs[i].driveMotor.velocity, Rotation2d.fromRotations(moduleInputs[i].steerMotor.position));
         }
         Logger.recordOutput("Drive/Wanted States", wantedStates);
         Logger.recordOutput("Drive/Real States", realStates);

diff --git a/src/main/java/org/codeorange/frc2024/subsystem/drive/ModuleIO.java b/src/main/java/org/codeorange/frc2024/subsystem/drive/ModuleIO.java
@@ -1,26 +1,15 @@
 package org.codeorange.frc2024.subsystem.drive;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import org.codeorange.frc2024.utility.logging.MotorInputs;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ModuleIO {
     @AutoLog
     class ModuleInputs {
-        public double driveMotorPosition;
-        public double driveMotorVelocity;
-        public double driveMotorAcceleration;
-        public double driveMotorVoltage;
-        public double driveMotorAmps;
-        public double driveMotorTemp;
-
+        MotorInputs driveMotor;
         public double steerMotorAbsolutePosition;
-        public double steerMotorRelativePosition;
-        // omega
-        public double steerMotorVelocity;
-        public double steerMotorVoltage;
-        public double steerMotorAmps;
-        public double steerMotorTemp;
-
+        MotorInputs steerMotor;
 
         public double[] odometryTimestamps = new double[] {};
         public double[] odometryDrivePositionsMeters = new double[] {};