From ed76938d2be28067b5ba5fada616d29a8d1db8b2 Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Fri, 23 Jan 2026 19:27:38 -0800 Subject: [PATCH] I learned advantagekit + shooter IO logging --- .../frc/robot/subsystems/shooter/Shooter.java | 21 ++++++++++++++++--- .../robot/subsystems/shooter/ShooterIO.java | 17 +++++++++++++++ 2 files changed, 35 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIO.java diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c6051c4..01fb039 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -16,8 +16,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; +import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs;; -public class Shooter extends SubsystemBase { +public class Shooter extends SubsystemBase implements ShooterIO { private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.RIO_CAN); private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.RIO_CAN); @@ -32,6 +33,8 @@ public class Shooter extends SubsystemBase { //Velocity in rotations per second VelocityVoltage voltageRequest = new VelocityVoltage(0); + private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); + public Shooter(){ TalonFXConfiguration config = new TalonFXConfiguration(); config.Slot0.kP = 0.2; //tune p value @@ -63,6 +66,8 @@ public class Shooter extends SubsystemBase { } public void periodic(){ + updateInputs(); + shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); feederMotor.set(feederPower); @@ -86,9 +91,19 @@ public class Shooter extends SubsystemBase { } public double getFeederVelocity() { - return feederMotor.getVelocity().getValueAsDouble(); + return inputs.feederVelocity; } + public double getShooterVelcoity() { - return shooterMotorLeft.getVelocity().getValueAsDouble(); + return inputs.leftShooterVelocity; // assuming they are the same rn + } + + public void updateInputs() { + inputs.leftShooterVelocity = shooterMotorLeft.getVelocity().getValueAsDouble(); + inputs.rightShooterVelocity = shooterMotorRight.getVelocity().getValueAsDouble(); + inputs.feederVelocity = feederMotor.getVelocity().getValueAsDouble(); + inputs.leftShooterCurrent = shooterMotorLeft.getStatorCurrent().getValueAsDouble(); + inputs.rightShooterVelocity = shooterMotorRight.getStatorCurrent().getValueAsDouble(); + inputs.feederVelocity = feederMotor.getStatorCurrent().getValueAsDouble(); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java new file mode 100644 index 0000000..00585ff --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterIO { + @AutoLog + public static class ShooterIOInputs { + public double rightShooterVelocity = 0; + public double leftShooterVelocity = 0; + public double feederVelocity = 0; + public double rightShooterCurrent = 0; + public double leftShooterCurrent = 0; + public double feederCurrent = 0; + } + + public void updateInputs(); +} -- 2.39.5