diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 6b437179..8593c422 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/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() { diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java index fee2cc92..992abcec 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIO.java +++ b/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) { diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIOReal.java b/src/main/java/frc/robot/subsystems/hood/HoodIOReal.java index 60ed2a11..dc48e0e0 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIOReal.java +++ b/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()); } diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java b/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java index 6fb6c86b..f929933b 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java +++ b/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; } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 421dc1e0..7f0b15ad 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/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) { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index a43cd8ac..ec136760 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/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()); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 68ca604e..9ec68b4b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/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())); } diff --git a/src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIO.java b/src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIO.java index 58299e2b..0d77f3d9 100644 --- a/src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIO.java +++ b/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) { diff --git a/src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIOReal.java b/src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIOReal.java index 6b7f6fc3..15e2d879 100644 --- a/src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIOReal.java +++ b/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 } } diff --git a/src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIOSim.java b/src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIOSim.java index 5d9c87fb..d681b015 100644 --- a/src/main/java/frc/robot/subsystems/intakeRollers/IntakeRollerIOSim.java +++ b/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 } } diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIO.java b/src/main/java/frc/robot/subsystems/kicker/KickerIO.java index 894208be..bdd4ae2f 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIO.java +++ b/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) { diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java index 182f3507..88689721 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java +++ b/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 } } diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIOSim.java b/src/main/java/frc/robot/subsystems/kicker/KickerIOSim.java index a0a3f9da..2cd75225 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIOSim.java +++ b/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; + } } diff --git a/src/main/java/frc/robot/subsystems/module/Module.java b/src/main/java/frc/robot/subsystems/module/Module.java index bd38ae24..59825f1f 100644 --- a/src/main/java/frc/robot/subsystems/module/Module.java +++ b/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; + } } diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/module/ModuleIO.java index 7d713737..72f3cc0c 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIO.java +++ b/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[]{}; diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java index 56e5b88b..01f0b1c2 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java +++ b/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(); diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java index 5d673bd2..79b0959b 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 34aff352..1134aed2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/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; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 44143be3..92311eec 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/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 } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index c36d28ad..a09982eb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/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(); } } diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java index 02b4788d..34452567 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java +++ b/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) { diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOReal.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOReal.java index 5f5e9521..5dca8a33 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOReal.java +++ b/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; } } diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOSim.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOSim.java index 0563d0e8..1995539e 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOSim.java +++ b/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; + } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index f8203c6a..b26a8210 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/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) { diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOReal.java b/src/main/java/frc/robot/subsystems/turret/TurretIOReal.java index 7194fbdd..6da8f9ce 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOReal.java +++ b/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()); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java b/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java index c1f37407..52ffa4b7 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java +++ b/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; + } }