From d087f2f22021fd08f33bc5f3aa0eae2a130f2471 Mon Sep 17 00:00:00 2001 From: apembleton Date: Thu, 22 Jan 2026 17:55:36 -0800 Subject: [PATCH 1/6] started TurretIO --- .../robot/subsystems/shooter/TurretIO.java | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter/TurretIO.java diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java new file mode 100644 index 0000000..8ec526d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; + +/** Add your docs here. */ +public class TurretIO { + @AutoLog + public static class TurretIOInputs { + public Rotation2d turretPositionRotations = new Rotation2d(); + public double turretAngularVelocity = 0.0; + public double turretStatorCurrentAmps = 0.0; + public double turretSupplyCurrentAmps = 0.0; + public double turretVoltage = 0.0; + public double turretTempC = 0.0; + //_TODO: Input reall values + } + protected TalonFX turretMotor; + private final BaseStatusSignal turretPositionRotations; + private final BaseStatusSignal turretAngularVelocity; + private final StatusSignal turretVoltage; + private final StatusSignal turretStatorCurrent; + private final StatusSignal turretSupplyCurrent; + private final StatusSignal turretTemp; + +} From cce0e0cae6202698dbe03477e830f253091cefa3 Mon Sep 17 00:00:00 2001 From: apembleton Date: Thu, 29 Jan 2026 16:44:36 -0800 Subject: [PATCH 2/6] Added to TurretIO --- .../subsystems/shooter/ShooterSubsystem.java | 11 +-- .../robot/subsystems/shooter/TurretIO.java | 90 ++++++++++++++++++- 2 files changed, 94 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 9cfde0a..c2fbe6b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -25,10 +25,11 @@ public class ShooterSubsystem extends SubsystemBase { public static double HOOD_GEAR_RATIO = 147.0 / 13.0; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(0); + public static double TURRET_GEAR_RATIO = (12.0/42.0)*(16.0/32.0)*(10.0/85.0); public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; - public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; // TODO: TUNE + public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; HoodIO hoodIO; HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); @@ -51,7 +52,7 @@ public class ShooterSubsystem extends SubsystemBase { (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state)), new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); - /** Creates a new HoodSubsystem. */ + public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { this.hoodIO = hoodIO; this.flywheelIO = flywheelIO; @@ -84,7 +85,7 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar public Command rest() { return this.run( () -> { - hoodIO.setHoodPosition(Rotation2d.kZero); // TODO: TUNE TUCKED POSITION IF NEEDED + hoodIO.setHoodPosition(Rotation2d.kZero); flywheelIO.setFlywheelVoltage(0.0); }); } @@ -94,7 +95,7 @@ public Command spit() { () -> { hoodIO.setHoodPosition(Rotation2d.kZero); flywheelIO.setMotionProfiledFlywheelVelocity(20); - }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY + }); } public Command setHoodPositionCommand(Supplier hoodPosition) { @@ -117,7 +118,7 @@ public Command runHoodSysid() { .until( () -> hoodInputs.hoodPositionRotations.getDegrees() - > (HOOD_MAX_ROTATION.getDegrees() - 5)), // Stop before endstop + > (HOOD_MAX_ROTATION.getDegrees() - 5)), hoodSysid .quasistatic(Direction.kReverse) .until( diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index 8ec526d..0559a90 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -7,10 +7,18 @@ import org.littletonrobotics.junction.AutoLog; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; @@ -20,7 +28,6 @@ public class TurretIO { @AutoLog public static class TurretIOInputs { public Rotation2d turretPositionRotations = new Rotation2d(); - public double turretAngularVelocity = 0.0; public double turretStatorCurrentAmps = 0.0; public double turretSupplyCurrentAmps = 0.0; public double turretVoltage = 0.0; @@ -29,10 +36,89 @@ public static class TurretIOInputs { } protected TalonFX turretMotor; private final BaseStatusSignal turretPositionRotations; - private final BaseStatusSignal turretAngularVelocity; private final StatusSignal turretVoltage; private final StatusSignal turretStatorCurrent; private final StatusSignal turretSupplyCurrent; private final StatusSignal turretTemp; + private final StatusSignal turretAngularVelocity; + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); + private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); + + public TurretIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { + turretMotor = new TalonFX(11, canbus); + turretMotor.getConfigurator().apply(TurretIO.getTurretConfiguration()); + turretPositionRotations = turretMotor.getPosition(); + turretAngularVelocity = turretMotor.getVelocity(); + turretVoltage = turretMotor.getMotorVoltage(); + turretStatorCurrent = turretMotor.getStatorCurrent(); + turretSupplyCurrent = turretMotor.getSupplyCurrent(); + turretTemp = turretMotor.getDeviceTemp(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + turretPositionRotations, + turretAngularVelocity, + turretVoltage, + turretStatorCurrent, + turretSupplyCurrent, + turretTemp); + turretMotor.optimizeBusUtilization(); + } + + + public static TalonFXConfiguration getTurretConfiguration() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.Feedback.SensorToMechanismRatio = ShooterSubsystem.TURRET_GEAR_RATIO; + + // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; Potentially need, maybe not tho. + + config.Slot0.kS = 0.0; + config.Slot0.kG = 0.0; + config.Slot0.kV = 1.1; + config.Slot0.kP = 5.0; + config.Slot0.kD = 0.0; + + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + + return config; +} + public void setTurretVoltage(double turretVoltage) { + turretMotor.setControl(voltageOut.withOutput(turretVoltage)); + } + + public void setturretPosition(Rotation2d turretPosition) { + turretMotor.setControl(positionVoltage.withPosition(turretPosition.getRotations())); + } + + public void setTurretVelocity(double turretVelocity) { + turretMotor.setControl(velocityVoltage.withVelocity(turretVelocity)); + } + + public void updateInputs(TurretIOInputs inputs) { + BaseStatusSignal.refreshAll( + turretPositionRotations, + turretVoltage, + turretStatorCurrent, + turretSupplyCurrent, + turretTemp); + + inputs.turretPositionRotations = Rotation2d.fromRadians(turretPositionRotations.getValueAsDouble()); + inputs.turretVoltage = turretVoltage.getValueAsDouble(); + inputs.turretStatorCurrentAmps = turretStatorCurrent.getValueAsDouble(); + inputs.turretSupplyCurrentAmps = turretSupplyCurrent.getValueAsDouble(); + inputs.turretTempC = turretTemp.getValueAsDouble(); + } } From bfad163afd9c222d77c7c62bf8fa87664c60f2db Mon Sep 17 00:00:00 2001 From: apembleton Date: Thu, 29 Jan 2026 18:12:27 -0800 Subject: [PATCH 3/6] Filled out Turret command --- src/main/java/frc/robot/Robot.java | 4 +- .../subsystems/shooter/ShooterSubsystem.java | 4 +- .../robot/subsystems/shooter/TurretIO.java | 91 +++++++++---------- .../subsystems/shooter/TurretSubsystem.java | 51 ++++++++--- 4 files changed, 90 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a94a93f..2983780 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; @@ -45,6 +46,7 @@ import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.shooter.TurretIO; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; @@ -277,7 +279,7 @@ public Robot() { case COMP: indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); - shooter = new TurretSubsystem(); + shooter = new TurretSubsystem(new TurretIO(TurretIO.getTurretConfiguration(), canivore)); climber = new ClimberSubsystem(); // TODO climber break; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index c4a28de..288929b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -28,7 +28,7 @@ public class ShooterSubsystem extends SubsystemBase implements Shooter { public static double HOOD_GEAR_RATIO = 24.230769; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); - public static double TURRET_GEAR_RATIO = (12.0/42.0)*(16.0/32.0)*(10.0/85.0); + public static double TURRET_GEAR_RATIO = (12.0 / 42.0) * (16.0 / 32.0) * (10.0 / 85.0); public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; @@ -140,7 +140,7 @@ public Command runHoodSysid() { .until( () -> hoodInputs.hoodPositionRotations.getDegrees() - > (HOOD_MAX_ROTATION.getDegrees() - 5)), + > (HOOD_MAX_ROTATION.getDegrees() - 5)), hoodSysid .quasistatic(Direction.kReverse) .until( diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index 0559a90..98a8ee6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -4,8 +4,6 @@ package frc.robot.subsystems.shooter; -import org.littletonrobotics.junction.AutoLog; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; @@ -16,59 +14,58 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.AutoLog; /** Add your docs here. */ public class TurretIO { - @AutoLog - public static class TurretIOInputs { - public Rotation2d turretPositionRotations = new Rotation2d(); - public double turretStatorCurrentAmps = 0.0; - public double turretSupplyCurrentAmps = 0.0; - public double turretVoltage = 0.0; - public double turretTempC = 0.0; - //_TODO: Input reall values - } - protected TalonFX turretMotor; - private final BaseStatusSignal turretPositionRotations; - private final StatusSignal turretVoltage; - private final StatusSignal turretStatorCurrent; - private final StatusSignal turretSupplyCurrent; - private final StatusSignal turretTemp; - private final StatusSignal turretAngularVelocity; - private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + @AutoLog + public static class TurretIOInputs { + public Rotation2d turretPositionRotations = new Rotation2d(); + public double turretStatorCurrentAmps = 0.0; + public double turretSupplyCurrentAmps = 0.0; + public double turretVoltage = 0.0; + public double turretTempC = 0.0; + // _TODO: Input reall values + } + + protected TalonFX turretMotor; + private final BaseStatusSignal turretPositionRotations; + private final StatusSignal turretVoltage; + private final StatusSignal turretStatorCurrent; + private final StatusSignal turretSupplyCurrent; + private final StatusSignal turretTemp; + private final StatusSignal turretAngularVelocity; + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); - public TurretIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { + public TurretIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { turretMotor = new TalonFX(11, canbus); turretMotor.getConfigurator().apply(TurretIO.getTurretConfiguration()); - turretPositionRotations = turretMotor.getPosition(); - turretAngularVelocity = turretMotor.getVelocity(); - turretVoltage = turretMotor.getMotorVoltage(); - turretStatorCurrent = turretMotor.getStatorCurrent(); - turretSupplyCurrent = turretMotor.getSupplyCurrent(); - turretTemp = turretMotor.getDeviceTemp(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - turretPositionRotations, - turretAngularVelocity, - turretVoltage, - turretStatorCurrent, - turretSupplyCurrent, - turretTemp); - turretMotor.optimizeBusUtilization(); - } - - - - public static TalonFXConfiguration getTurretConfiguration() { + turretPositionRotations = turretMotor.getPosition(); + turretAngularVelocity = turretMotor.getVelocity(); + turretVoltage = turretMotor.getMotorVoltage(); + turretStatorCurrent = turretMotor.getStatorCurrent(); + turretSupplyCurrent = turretMotor.getSupplyCurrent(); + turretTemp = turretMotor.getDeviceTemp(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + turretPositionRotations, + turretAngularVelocity, + turretVoltage, + turretStatorCurrent, + turretSupplyCurrent, + turretTemp); + turretMotor.optimizeBusUtilization(); + } + + public static TalonFXConfiguration getTurretConfiguration() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -94,12 +91,13 @@ public static TalonFXConfiguration getTurretConfiguration() { config.CurrentLimits.SupplyCurrentLimit = 60.0; return config; -} - public void setTurretVoltage(double turretVoltage) { + } + + public void setTurretVoltage(double turretVoltage) { turretMotor.setControl(voltageOut.withOutput(turretVoltage)); } - public void setturretPosition(Rotation2d turretPosition) { + public void setTurretPosition(Rotation2d turretPosition) { turretMotor.setControl(positionVoltage.withPosition(turretPosition.getRotations())); } @@ -115,7 +113,8 @@ public void updateInputs(TurretIOInputs inputs) { turretSupplyCurrent, turretTemp); - inputs.turretPositionRotations = Rotation2d.fromRadians(turretPositionRotations.getValueAsDouble()); + inputs.turretPositionRotations = + Rotation2d.fromRadians(turretPositionRotations.getValueAsDouble()); inputs.turretVoltage = turretVoltage.getValueAsDouble(); inputs.turretStatorCurrentAmps = turretStatorCurrent.getValueAsDouble(); inputs.turretSupplyCurrentAmps = turretSupplyCurrent.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 685b95a..3031271 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -5,42 +5,67 @@ package frc.robot.subsystems.shooter; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { + public TurretIO turretIO; + public TurretIOInputsAutoLogged turretIOInputs = new TurretIOInputsAutoLogged(); + /** Creates a new TurretSubsystem. */ - public TurretSubsystem() {} + public TurretSubsystem(TurretIO turretIO) { + this.turretIO = turretIO; + } @Override public void periodic() { + turretIO.updateInputs(turretIOInputs); + Logger.processInputs("Shooter/Turret", turretIOInputs); // This method will be called once per scheduler run } @Override public Command shoot(Supplier robotPoseSupplier) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'shoot'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } @Override public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'feed'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } @Override public Command rest() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'rest'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } @Override public Command spit() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'spit'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } @Override @@ -63,7 +88,11 @@ public Command zeroHood() { @Override public Command testShoot() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'testShoot'"); + return this.run( + () -> { + turretIO.setTurretPosition(new Rotation2d()); + // TODO:Find the reall number + + }); } } From a693e613a836b360335c6075146e40baf85a393a Mon Sep 17 00:00:00 2001 From: apembleton Date: Sun, 1 Feb 2026 13:54:47 -0800 Subject: [PATCH 4/6] fixed errors in TurretIO --- src/main/java/frc/robot/subsystems/shooter/TurretIO.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index 98a8ee6..e63df2b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -27,10 +27,10 @@ public class TurretIO { public static class TurretIOInputs { public Rotation2d turretPositionRotations = new Rotation2d(); public double turretStatorCurrentAmps = 0.0; + public double velocityRotationsPerSec = 0.0; public double turretSupplyCurrentAmps = 0.0; public double turretVoltage = 0.0; public double turretTempC = 0.0; - // _TODO: Input reall values } protected TalonFX turretMotor; From a64a601520eb3ab8e5ba0409f5482844b3e3300d Mon Sep 17 00:00:00 2001 From: apembleton Date: Sun, 1 Feb 2026 14:34:19 -0800 Subject: [PATCH 5/6] Created TurretIOSim --- .../subsystems/shooter/ShooterSubsystem.java | 1 - .../robot/subsystems/shooter/TurretIO.java | 2 +- .../robot/subsystems/shooter/TurretIOSim.java | 46 +++++++++++++++++++ .../subsystems/shooter/TurretSubsystem.java | 2 + 4 files changed, 49 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 288929b..ea0187a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -28,7 +28,6 @@ public class ShooterSubsystem extends SubsystemBase implements Shooter { public static double HOOD_GEAR_RATIO = 24.230769; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); - public static double TURRET_GEAR_RATIO = (12.0 / 42.0) * (16.0 / 32.0) * (10.0 / 85.0); public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index e63df2b..a871211 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -76,7 +76,7 @@ public static TalonFXConfiguration getTurretConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.SensorToMechanismRatio = ShooterSubsystem.TURRET_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = TurretSubsystem.TURRET_GEAR_RATIO; // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; Potentially need, maybe not tho. diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java new file mode 100644 index 0000000..850f20f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class TurretIOSim extends TurretIO{ + DCMotorSim turretphysicssSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 0.001, TurretSubsystem.TURRET_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)); + +TalonFXSimState motorSim; +final double simLoopPeriod = 0.002; +Notifier simNotifier; +double lastSimTime = 0.0; + +public TurretIOSim(TalonFXConfiguration configuration, CANBus canBus){ + super(configuration, canBus); +motorSim = turretMotor.getSimState(); +motorSim.setMotorType(MotorType.KrakenX44); +motorSim.Orientation = ChassisReference.Clockwise_Positive; +simNotifier = new Notifier(() -> { + double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; +motorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); +turretphysicssSim.setInputVoltage(motorSim.getMotorVoltage()); +turretphysicssSim.update(deltaTime); +motorSim.setRawRotorPosition(turretphysicssSim.getAngularPositionRotations() * turretphysicssSim.getGearing()); +motorSim.setRotorVelocity(turretphysicssSim.getAngularVelocityRPM()/60 * turretphysicssSim.getGearing()); + +}); +simNotifier.startPeriodic(simLoopPeriod); + +} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 3031271..be3cc2f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -11,8 +11,10 @@ import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; + /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { + public static double TURRET_GEAR_RATIO = (12.0 / 42.0) * (16.0 / 32.0) * (10.0 / 85.0); public TurretIO turretIO; public TurretIOInputsAutoLogged turretIOInputs = new TurretIOInputsAutoLogged(); From 1104b2d9c9cddd05e4cb82f3d16266990fb936c0 Mon Sep 17 00:00:00 2001 From: apembleton Date: Sun, 1 Feb 2026 14:38:01 -0800 Subject: [PATCH 6/6] Added TurretIOSim to Robot.java --- src/main/java/frc/robot/Robot.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2983780..c21e002 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -47,6 +47,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.shooter.TurretIO; +import frc.robot.subsystems.shooter.TurretIOSim; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; @@ -279,7 +280,12 @@ public Robot() { case COMP: indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); - shooter = new TurretSubsystem(new TurretIO(TurretIO.getTurretConfiguration(), canivore)); + shooter = + new TurretSubsystem( + ROBOT_MODE == RobotMode.REAL + ? new TurretIO(TurretIO.getTurretConfiguration(), canivore) + : new TurretIOSim(TurretIO.getTurretConfiguration(), canivore) + ); climber = new ClimberSubsystem(); // TODO climber break; }