From 377d9f2b8fc2ab842649a219fc3c3959affe03d3 Mon Sep 17 00:00:00 2001 From: georgelin13 Date: Thu, 1 Feb 2024 19:17:53 -0500 Subject: [PATCH] feat: finished shooter IO autologged inputs --- .../robot/subsystems/shooter/ShooterIO.java | 25 +++++++++++++- .../shooter/ShooterIOPrototype.java | 33 +++++++++++++++---- .../subsystems/shooter/ShooterSubsystem.java | 2 ++ 3 files changed, 53 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 74ecd51b..871ab85c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -4,12 +4,35 @@ public interface ShooterIO { @AutoLog - class ShooterIOInputs {} + class ShooterIOInputs { + public double tlVelocity = 0.0; + public double trVelocity = 0.0; + public double blVelocity = 0.0; + public double brVelocity = 0.0; + public double tlAppliedOutput = 0.0; + public double trAppliedOutput = 0.0; + public double blAppliedOutput = 0.0; + public double brAppliedOutput = 0.0; + public double kickerAppliedOutput = 0.0; + public double tlCurrentDraw = 0.0; + public double trCurrentDraw = 0.0; + public double blCurrentDraw = 0.0; + public double brCurrentDraw = 0.0; + public double kickerCurrentDraw = 0.0; + public double tlTemperature = 0.0; + public double trTemperature = 0.0; + public double blTemperature = 0.0; + public double brTemperature = 0.0; + + + + } default void setMotorVoltageTL(double voltage) {} default void setMotorVoltageTR(double voltage) {} default void setMotorVoltageBL(double voltage) {} default void setMotorVoltageBR(double voltage) {} default void setKickerVoltage(double voltage) {} default void setIntakeVoltage(double voltage) {} + default void updateInputs(ShooterIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java index e5a4157d..70a51afd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java @@ -9,30 +9,30 @@ public class ShooterIOPrototype implements ShooterIO { private final CANSparkMax m_topRightMotor; private final CANSparkFlex m_bottomLeftMotor; private final CANSparkMax m_bottomRightMotor; - private final CANSparkMax m_kickekMotor; + private final CANSparkMax m_kickerkMotor; public ShooterIOPrototype() { m_topLeftMotor = new CANSparkFlex(13, CANSparkLowLevel.MotorType.kBrushless); m_topRightMotor = new CANSparkMax(14, CANSparkLowLevel.MotorType.kBrushless); m_bottomLeftMotor = new CANSparkFlex(15, CANSparkLowLevel.MotorType.kBrushless); m_bottomRightMotor = new CANSparkMax(16, CANSparkLowLevel.MotorType.kBrushless); - m_kickekMotor = new CANSparkMax(17, CANSparkLowLevel.MotorType.kBrushless); + m_kickerkMotor = new CANSparkMax(17, CANSparkLowLevel.MotorType.kBrushless); m_topLeftMotor.setInverted(false); m_topRightMotor.setInverted(true); m_bottomLeftMotor.setInverted(true); m_bottomRightMotor.setInverted(true); - m_kickekMotor.setInverted(true); + m_kickerkMotor.setInverted(true); m_topLeftMotor.burnFlash(); m_topRightMotor.burnFlash(); m_topRightMotor.burnFlash(); m_bottomRightMotor.burnFlash(); - m_kickekMotor.burnFlash(); + m_kickerkMotor.burnFlash(); } // @Override // public void updateInputs(ShooterIOInputs inputs) { -// inputs.tLAngularVelocity = m_topLeftMotor.getEncoder().getVelocity() * Math.PI; +// inputs.tLAngularVelocity = m_topLeftMotor.getEncoder().getVelocity() * Math.PI * 2; // inputs.tRAngularVelocity = m_topRightMotor.getEncoder().getVelocity() * Math.PI; // inputs.bLAngularVelocity = m_bottomLeftMotor.getEncoder().getVelocity() * Math.PI; // inputs.bRAngularVelocity = m_bottomRightMotor.getEncoder().getVelocity() * Math.PI; @@ -67,7 +67,28 @@ public void setMotorVoltageBR(double voltage) { @Override public void setKickerVoltage(double voltage) { - m_kickekMotor.setVoltage(voltage); + m_kickerkMotor.setVoltage(voltage); } + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.tlVelocity = m_topLeftMotor.getEncoder().getVelocity() * Math.PI * 2.0; + inputs.trVelocity = m_topRightMotor.getEncoder().getVelocity() * Math.PI * 2.0; + inputs.blVelocity = m_bottomLeftMotor.getEncoder().getVelocity() * Math.PI * 2.0; + inputs.brVelocity = m_bottomRightMotor.getEncoder().getVelocity() * Math.PI * 2.0; + inputs.tlAppliedOutput = m_topLeftMotor.getAppliedOutput(); + inputs.trAppliedOutput = m_topRightMotor.getAppliedOutput(); + inputs.blAppliedOutput = m_bottomLeftMotor.getAppliedOutput(); + inputs.brAppliedOutput = m_bottomRightMotor.getAppliedOutput(); + inputs.kickerAppliedOutput = m_kickerkMotor.getAppliedOutput(); + inputs.tlCurrentDraw = m_topLeftMotor.getOutputCurrent(); + inputs.trCurrentDraw = m_topRightMotor.getOutputCurrent(); + inputs.blCurrentDraw = m_bottomLeftMotor.getOutputCurrent(); + inputs.brCurrentDraw = m_bottomRightMotor.getOutputCurrent(); + inputs.kickerCurrentDraw = m_kickerkMotor.getOutputCurrent(); + inputs.tlTemperature = m_topLeftMotor.getMotorTemperature(); + inputs.trTemperature = m_topRightMotor.getMotorTemperature(); + inputs.blTemperature = m_bottomLeftMotor.getMotorTemperature(); + inputs.brTemperature = m_bottomRightMotor.getMotorTemperature(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index c315aa43..c9ed37e7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -7,10 +7,12 @@ public class ShooterSubsystem extends SubsystemBase { private final ShooterIO m_io; + private final ShooterIOInputsAutoLogged m_inputs; private static final String TOP_WHEEL_RATIO = "Top wheel ratio"; public ShooterSubsystem(ShooterIO io) { m_io = io; + m_inputs = new ShooterIOInputsAutoLogged(); SmartDashboard.putNumber(TOP_WHEEL_RATIO, 0.8); SmartDashboard.putNumber("Left wheel", 0.6); SmartDashboard.putNumber("Right wheel", 0.6);