Power Logging

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

@@ -137,12 +137,22 @@ public void periodic() {
 
     double totalDriveCurrent = 0;
     double totalSteerCurrent = 0;
+    double totalDrawJoules = 0;
+    double totalDriveWattage = 0;
+    double totalSteerWattage = 0;
     for (Module module : modules) {
       totalDriveCurrent += module.getDriveCurrent();
       totalSteerCurrent += module.getSteerCurrent();
+      totalDrawJoules += module.getTotalDrawJoules();
+      totalDriveWattage += module.getDriveWattage();
+      totalSteerWattage += module.getSteerWattage();
     }
+
     Logger.recordOutput("Subsystems/Drive/totalDriveCurrent", totalDriveCurrent);
     Logger.recordOutput("Subsystems/Drive/totalSteerCurrent", totalSteerCurrent);
+    Logger.recordOutput("Subsystems/Drive/totalDrivePower", totalDriveWattage);
+    Logger.recordOutput("Subsystems/Drive/totalSteerPower", totalSteerWattage);
+    Logger.recordOutput("Subsystems/Drive/totalEnergy", totalDrawJoules);
   }
 
   private void stop() {

src/main/java/frc/robot/subsystems/hood/HoodIO.java

@@ -18,6 +18,8 @@ public class HoodIOInputs {
     public double motorCurrent;
     public double motorVoltage;
     public double motorTemperatureCelsius;
+    public double motorTotalEnergy = 0;
+    public double motorPower;
   }
 
   public default void setAngleRadians(double angle) {

src/main/java/frc/robot/subsystems/hood/HoodIOReal.java

@@ -59,7 +59,8 @@ public void updateInputs(HoodIOInputs inputs) {
     inputs.motorCurrent = m_motor.getOutputCurrent();
     inputs.motorVoltage = m_motor.getAppliedOutput() * m_motor.getBusVoltage();
     inputs.motorTemperatureCelsius = m_motor.getMotorTemperature();
-
+    inputs.motorTotalEnergy = inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorCurrent * inputs.motorVoltage;
     Logger.recordOutput("Subsystems/Hood/encoderPositionRaw", m_encoder.getPosition());
   }
 

src/main/java/frc/robot/subsystems/hood/HoodIOSim.java

@@ -51,6 +51,8 @@ public void updateInputs(HoodIOInputs inputs) {
     inputs.angularVelocityDegreesPerSec = inputs.angularVelocityRadPerSec * 180 / Math.PI;
     inputs.motorCurrent = m_motor.getCurrentDrawAmps();
     inputs.motorVoltage = m_motor.getInputVoltage();
+    inputs.motorTotalEnergy = inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorCurrent * inputs.motorVoltage;
     inputs.motorTemperatureCelsius = 0;
   }
 }

src/main/java/frc/robot/subsystems/intake/IntakeIO.java

@@ -12,6 +12,8 @@ public static class IntakeIOInputs {
     public double velocityRadPerSec;
     public double positionDegrees;
     public double velocityDegreesPerSec;
+    public double motorTotalEnergy = 0;
+    public double motorPower;
   }
 
   public default void updateInputs(IntakeIOInputs inputs) {

src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java

@@ -107,7 +107,8 @@ public void updateInputs(IntakeIOInputs inputs) {
     inputs.velocityRadPerSec = getVelocityRadPerSec(); // rad/s
     inputs.positionDegrees = inputs.positionRadians * 180 / Math.PI; // degrees
     inputs.velocityDegreesPerSec = inputs.velocityRadPerSec * 180 / Math.PI; // deg/s
-
+    inputs.motorTotalEnergy += inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorVoltage * inputs.motorCurrent; // W
     Logger.recordOutput("Subsystems/Intake/encoderPositionRaw", m_encoder.getPosition());
   }
 }

src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java

@@ -51,6 +51,8 @@ public void updateInputs(IntakeIOInputs inputs) {
     inputs.velocityRadPerSec = m_motor.getAngularVelocityRadPerSec(); // rad/s
     inputs.positionDegrees = inputs.positionRadians * 180 / Math.PI;
     inputs.velocityDegreesPerSec = inputs.velocityRadPerSec * 180 / Math.PI;
+    inputs.motorTotalEnergy += inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorVoltage * inputs.motorCurrent; // W
 
     intakeLigament.setAngle(90 - Units.radiansToDegrees(m_motor.getAngularPositionRad()));
   }

src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIO.java

@@ -9,6 +9,8 @@ public static class IntakeRollerIOInputs {
     public double motorTemperatureCelsius;
     public double motorCurrent;
     public double encoderVelocityRadiansPerSecond;
+    public double motorTotalEnergy = 0;
+    public double motorPower;
   }
 
   public default void updateInputs(IntakeRollerIOInputs inputs) {

src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIOReal.java

@@ -45,5 +45,7 @@ public void updateInputs(IntakeRollerIOInputs inputs) {
     inputs.motorCurrent = m_motor.getOutputCurrent(); // amps
     inputs.motorVoltage = m_motor.getAppliedOutput() * m_motor.getBusVoltage(); // volts
     inputs.encoderVelocityRadiansPerSecond = m_encoder.getVelocity() * 2 * Math.PI / 60; // rad/s
+    inputs.motorTotalEnergy = inputs.motorCurrent * inputs.motorVoltage * 0.02; // J
+    inputs.motorPower = inputs.motorVoltage * inputs.motorCurrent; // W
   }
 }

src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIOSim.java

@@ -41,5 +41,7 @@ public void updateInputs(IntakeRollerIOInputs inputs) {
     inputs.motorCurrent = m_motor.getCurrentDrawAmps(); // amps
     inputs.motorVoltage = m_motor.getInputVoltage(); // volts
     inputs.encoderVelocityRadiansPerSecond = m_motor.getAngularVelocityRadPerSec(); // rad/s
+    inputs.motorTotalEnergy = inputs.motorCurrent * inputs.motorVoltage * 0.02; // J
+    inputs.motorPower = inputs.motorVoltage * inputs.motorCurrent; // W
   }
 }

src/main/java/frc/robot/subsystems/kicker/KickerIO.java

@@ -10,6 +10,8 @@ public class KickerIOInputs {
     public double motorTemperatureCelsius;
     public double motorVelocityRadPerSec;
     public double motorVelocityRPM;
+    public double motorTotalEnergy = 0;
+    public double motorPower;
   }
 
   public default void setVelocity(double velocity) {

src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java

@@ -39,5 +39,8 @@ public void updateInputs(KickerIOInputs inputs) {
     inputs.motorTemperatureCelsius = m_motor.getMotorTemperature();
     inputs.motorVelocityRadPerSec = m_encoder.getVelocity() * 2 * Math.PI / 60;
     inputs.motorVelocityRPM = m_encoder.getVelocity();
+
+    inputs.motorTotalEnergy += inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorVoltage * inputs.motorCurrent; // W
   }
 }

src/main/java/frc/robot/subsystems/kicker/KickerIOSim.java

@@ -36,5 +36,8 @@ public void updateInputs(KickerIOInputs inputs) {
     inputs.motorTemperatureCelsius = 0;
     inputs.motorVelocityRadPerSec = m_motorSim.getAngularVelocityRadPerSec();
     inputs.motorVelocityRPM = m_motorSim.getAngularVelocityRPM();
+    inputs.motorTotalEnergy = inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorVoltage * inputs.motorCurrent;
+
   }
 }

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

@@ -158,4 +158,16 @@ public SwerveModulePosition getPosition() {
   public double getVelocity() {
     return inputs.driveVelocityMetersPerSecond;
   }
+
+  public double getTotalDrawJoules() {
+    return inputs.totalEnergy;
+  }
+
+  public double getDriveWattage() {
+    return inputs.drivePower;
+  }
+
+  public double getSteerWattage() {
+    return inputs.steerPower;
+  }
 }

src/main/java/frc/robot/subsystems/module/ModuleIO.java

@@ -26,7 +26,13 @@ public static class ModuleIOInputs {
     public double steerEncoderRawValue;
     public double steerEncoderRelative;
     // public int rawEncoderValue;
+    public double driveTotalEnergy = 0;
+    public double steerTotalEnergy = 0;
+    public double totalEnergy = 0;
 
+    public double drivePower;
+    public double steerPower;
+    public double totalPower;
     public double[] odometryTimestamps = new double[]{};
     public double[] odometryDrivePositionsMeters = new double[]{};
     public Rotation2d[] odometryTurnPositions = new Rotation2d[]{};

src/main/java/frc/robot/subsystems/module/ModuleIOReal.java

@@ -98,6 +98,13 @@ public void updateInputs(ModuleIOInputs inputs) {
             * DriveConstants.wheelRadius * 2 * Math.PI)
         .toArray();
 
+    inputs.driveTotalEnergy += inputs.driveAppliedVoltage * inputs.driveCurrentAmps * 0.02;
+    inputs.steerTotalEnergy += inputs.steerAppliedVoltage * inputs.steerCurrentAmps * 0.02;
+    inputs.totalEnergy = inputs.driveTotalEnergy + inputs.steerTotalEnergy;
+    inputs.drivePower = inputs.driveAppliedVoltage * inputs.driveCurrentAmps;
+    inputs.steerPower = inputs.steerAppliedVoltage * inputs.steerCurrentAmps;
+    inputs.totalPower = inputs.drivePower + inputs.steerPower;
+
     timestampQueue.clear();
     drivePositionQueue.clear();
     turnPositionQueue.clear();

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

@@ -86,6 +86,14 @@ public void updateInputs(ModuleIOInputs inputs) {
     inputs.odometryDrivePositionsMeters = new double[]{inputs.drivePositionMeters};
     inputs.odometryTurnPositions = new Rotation2d[]{Rotation2d.fromDegrees(inputs.steerAngleDegrees)};
     inputs.driveVelocities = new double[]{inputs.driveVelocityMetersPerSecond};
+
+    inputs.driveTotalEnergy += inputs.driveAppliedVoltage * inputs.driveCurrentAmps * 0.02;
+    inputs.steerTotalEnergy += inputs.steerAppliedVoltage * inputs.steerCurrentAmps * 0.02;
+    inputs.totalEnergy = inputs.driveTotalEnergy + inputs.steerTotalEnergy;
+    inputs.drivePower = inputs.driveAppliedVoltage * inputs.driveCurrentAmps;
+    inputs.steerPower = inputs.steerAppliedVoltage * inputs.steerCurrentAmps;
+    inputs.totalPower = inputs.drivePower + inputs.steerPower;
+
   }
 
   @Override

src/main/java/frc/robot/subsystems/shooter/ShooterIO.java

@@ -22,6 +22,14 @@ public class ShooterIOInputs {
     public double followerMotorTemperatureCelsius;
     public double followerMotorPositionRotations;
 
+    public double leaderTotalEnergy = 0;
+    public double followerTotalEnergy = 0;
+    public double totalEnergy = 0;
+
+    public double leaderPower;
+    public double followerPower;
+    public double totalPower;
+
     public double averageVoltage;
   }
 

src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java

@@ -81,6 +81,13 @@ public void updateInputs(ShooterIOInputs inputs) {
     inputs.followerMotorCurrent = m_followerMotor.getOutputCurrent(); // amps
     inputs.followerMotorPositionRotations = m_followerEncoder.getPosition(); // rotations
 
+    inputs.leaderTotalEnergy += inputs.leaderMotorVoltage * inputs.leaderMotorCurrent * 0.02; // J
+    inputs.followerTotalEnergy += inputs.followerMotorVoltage * inputs.followerMotorCurrent * 0.02; // J
+    inputs.totalEnergy = inputs.leaderTotalEnergy + inputs.followerTotalEnergy; // J
+    inputs.leaderPower = inputs.leaderMotorVoltage * inputs.leaderMotorCurrent; // W
+    inputs.followerPower = inputs.followerMotorVoltage * inputs.followerMotorCurrent; // W
+    inputs.totalPower = inputs.leaderPower + inputs.followerPower; // W
+
     inputs.averageVoltage = (inputs.leaderMotorVoltage + inputs.followerMotorVoltage) / 2.0; // rad/s
   }
 }

src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java

@@ -62,6 +62,13 @@ public void updateInputs(ShooterIOInputs inputs) {
     inputs.followerMotorTemperatureCelsius = 0;
     inputs.followerMotorPositionRotations = m_motor.getAngularPositionRotations();
 
+    inputs.leaderTotalEnergy += inputs.leaderMotorVoltage * inputs.leaderMotorCurrent * 0.02; // J
+    inputs.followerTotalEnergy += inputs.followerMotorVoltage * inputs.followerMotorCurrent * 0.02; // J
+    inputs.totalEnergy = inputs.leaderTotalEnergy + inputs.followerTotalEnergy; // J
+    inputs.leaderPower = inputs.leaderMotorVoltage * inputs.leaderMotorCurrent; // W
+    inputs.followerPower = inputs.followerMotorVoltage * inputs.followerMotorCurrent; // W
+    inputs.totalPower = inputs.leaderPower + inputs.followerPower; // W
+
     inputs.averageVoltage = m_motor.getInputVoltage();
   }
 }

src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java

@@ -11,6 +11,8 @@ public static class SpindexerIOInputs {
     public double motorCurrent = 0.0;
     public double motorTemperatureCelsius = 0.0;
     public boolean isJammed = false;
+    public double motorTotalEnergy = 0.0;
+    public double motorPower;
   }
 
   public default void updateInputs(SpindexerIOInputs inputs) {

src/main/java/frc/robot/subsystems/spindexer/SpindexerIOReal.java

@@ -49,5 +49,7 @@ public void updateInputs(SpindexerIOInputs inputs) {
     inputs.motorCurrent = m_motor.getOutputCurrent();
     inputs.motorTemperatureCelsius = m_motor.getMotorTemperature();
     inputs.isJammed = m_currentEma.get() > SpindexerConstants.jamCurrentThresh;
+    inputs.motorTotalEnergy += inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorCurrent * inputs.motorVoltage;
   }
 }

src/main/java/frc/robot/subsystems/spindexer/SpindexerIOSim.java

@@ -47,5 +47,8 @@ public void updateInputs(SpindexerIOInputs inputs) {
     inputs.motorCurrent = m_motorSim.getCurrentDrawAmps();
     inputs.motorTemperatureCelsius = 0.0;
     inputs.isJammed = false;
+    inputs.motorTotalEnergy += inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorCurrent * inputs.motorVoltage;
+
   }
 }

src/main/java/frc/robot/subsystems/turret/TurretIO.java

@@ -15,6 +15,8 @@ public class TurretIOInputs {
     public double motorCurrent;
     public double motorVoltage;
     public double motorTemperatureCelsius;
+    public double motorTotalEnergy = 0;
+    public double motorPower;
   }
 
   public default void setTurretState(ShotSetpoint setpoint) {

src/main/java/frc/robot/subsystems/turret/TurretIOReal.java

@@ -73,6 +73,8 @@ public void updateInputs(TurretIOInputs inputs) {
     inputs.motorCurrent = m_motor.getOutputCurrent();
     inputs.motorVoltage = m_motor.getBusVoltage() * m_motor.getAppliedOutput();
     inputs.motorTemperatureCelsius = m_motor.getMotorTemperature();
+    inputs.motorTotalEnergy += inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorCurrent * inputs.motorVoltage;
 
     Logger.recordOutput("Subsystems/Turret/encoderPositionRawVolts", m_encoder.getPosition());
     Logger.recordOutput("Subsystems/Turret/encoderVelocityRawVoltsPerSecond", m_encoder.getVelocity());

src/main/java/frc/robot/subsystems/turret/TurretIOSim.java

@@ -44,5 +44,8 @@ public void updateInputs(TurretIOInputs inputs) {
     inputs.motorCurrent = m_motor.getCurrentDrawAmps();
     inputs.motorVoltage = m_motor.getInputVoltage();
     inputs.motorTemperatureCelsius = 0;
+    inputs.motorTotalEnergy += inputs.motorCurrent * inputs.motorVoltage * 0.02;
+    inputs.motorPower = inputs.motorCurrent * inputs.motorVoltage;
+
   }
 }