From 7575b548c237f2a57f54a8a07391b4a07329fbff Mon Sep 17 00:00:00 2001 From: Justin Date: Sat, 29 Jul 2023 14:33:02 -0400 Subject: [PATCH] Add motor logging --- src/main/java/frc/robot/Constants.java | 2 +- .../robot/subsystems/elevatorIO/Elevator.java | 7 +++++ .../subsystems/elevatorIO/ElevatorIOSim.java | 4 +-- .../robot/subsystems/fourBarIO/FourBar.java | 4 +++ .../frc/robot/subsystems/intakeIO/Intake.java | 8 +++++- .../swerveIO/module/SwerveModule.java | 12 ++++++++- .../java/frc/robot/util/LoggableMotor.java | 27 +++++++++++++++++++ 7 files changed, 59 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/util/LoggableMotor.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2fc6ecc5..10f13785 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/elevatorIO/Elevator.java b/src/main/java/frc/robot/subsystems/elevatorIO/Elevator.java index 7241e0fd..4741c3fd 100644 --- a/src/main/java/frc/robot/subsystems/elevatorIO/Elevator.java +++ b/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)); diff --git a/src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIOSim.java index b0bb7d42..60a24891 100644 --- a/src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIOSim.java +++ b/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() { diff --git a/src/main/java/frc/robot/subsystems/fourBarIO/FourBar.java b/src/main/java/frc/robot/subsystems/fourBarIO/FourBar.java index 24ab898f..f92e0536 100644 --- a/src/main/java/frc/robot/subsystems/fourBarIO/FourBar.java +++ b/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) { diff --git a/src/main/java/frc/robot/subsystems/intakeIO/Intake.java b/src/main/java/frc/robot/subsystems/intakeIO/Intake.java index a4a5bc09..8c041765 100644 --- a/src/main/java/frc/robot/subsystems/intakeIO/Intake.java +++ b/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); diff --git a/src/main/java/frc/robot/subsystems/swerveIO/module/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerveIO/module/SwerveModule.java index 4c1770f7..5bd06e7b 100644 --- a/src/main/java/frc/robot/subsystems/swerveIO/module/SwerveModule.java +++ b/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( diff --git a/src/main/java/frc/robot/util/LoggableMotor.java b/src/main/java/frc/robot/util/LoggableMotor.java new file mode 100644 index 00000000..db24b563 --- /dev/null +++ b/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); + } +}