From 42e911fbdc7ad9af303bce96fb262829dfea7704 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 27 Jan 2026 18:13:51 -0700 Subject: [PATCH] Shoooterrr --- .../frc4388/robot/subsystems/TestRobot.java | 30 ++--- .../subsystems/shooter/ShooterConstants.java | 11 +- .../robot/subsystems/shooter/ShooterIO.java | 19 +-- .../robot/subsystems/shooter/ShooterReal.java | 120 +++++++++++------- 4 files changed, 110 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/TestRobot.java b/src/main/java/frc4388/robot/subsystems/TestRobot.java index 87faa80..c435c43 100644 --- a/src/main/java/frc4388/robot/subsystems/TestRobot.java +++ b/src/main/java/frc4388/robot/subsystems/TestRobot.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.utility.configurable.ConfigurableDouble; @@ -19,7 +20,7 @@ public class TestRobot extends SubsystemBase { // TalonFX m_intakeMotor; // TalonFX m_armMotor; - TalonFX m_intakeMotor; + // TalonFX m_storageMotor; TalonFX m_outerShooter; TalonFX m_innerShooter; @@ -50,19 +51,19 @@ public class TestRobot extends SubsystemBase { public TestRobot( // TalonFX intakeMotor, // TalonFX armMotor, - TalonFX intakeMotor, + // TalonFX storageMotor, TalonFX outerShooter, TalonFX innerShooter ) { // m_intakeMotor = intakeMotor; // m_armMotor = armMotor; - m_intakeMotor = intakeMotor; + // m_storageMotor = storageMotor; m_outerShooter = outerShooter; m_innerShooter = innerShooter; // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG); - m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); + // m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG); m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG); m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG); @@ -94,17 +95,16 @@ public class TestRobot extends SubsystemBase { // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means // ); - public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(40) // TODO: tune??? - .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny - .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - ); - + // public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 59ce39b..857bbeb 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -1,6 +1,6 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Degrees; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -16,6 +16,9 @@ public class ShooterConstants { public static final double PITCH_MOTOR_GEAR_RATIO = 1.; public static final double FLYWHEEL_GEAR_RATIO = 1.; public static final double FEEDER_INCHES_PER_ROT = 1.; + public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.; + public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.; + public static final double INDEXER_GEAR_RATIO = 1.; // Limits @@ -44,7 +47,7 @@ public class ShooterConstants { public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); } - public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? @@ -54,7 +57,7 @@ public class ShooterConstants { .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? @@ -64,7 +67,7 @@ public class ShooterConstants { .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 999abc3..1ea9a10 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -24,19 +24,22 @@ public interface ShooterIO { // Angle shooterTargetPitch = Rotations.of(0); // Current pitchMotorCurrent = Amps.of(0); - AngularVelocity flywheelVelocity = RotationsPerSecond.of(0); - AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0); - Current flywheelMotorCurrent = Amps.of(0); + AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0); + AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0); + AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0); + Current motor1Current = Amps.of(0); - LinearVelocity feederVelocity = InchesPerSecond.of(0); - LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); - Current feederMotorCurrent = Amps.of(0); + LinearVelocity motorLinearVelocity = InchesPerSecond.of(0); + AngularVelocity motorVelocity = RotationsPerSecond.of(0); + AngularVelocity motorTargetVelocity = RotationsPerSecond.of(0); + Current motor2Current = Amps.of(0); } // public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {} - public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {} - public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {} + public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {} + public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {} + public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {} public default void updateInputs(ShooterState state) {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 9db0e8b..0f35c0c 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -1,38 +1,40 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.*; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.configurable.ConfigurableDouble; -public class ShooterReal implements ShooterIO { +public class ShooterReal extends SubsystemBase implements ShooterIO { + + TalonFX m_shooter1Motor; + TalonFX m_shooter2Motor; + TalonFX m_indexerMotor; + VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0); + VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0); + VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0); - // TalonFX m_angleMotor; - // TalonFX m_pitchMotor; - TalonFX m_flywheelMotor; - TalonFX m_feederMotor; public ShooterReal( - // TalonFX angleMotor, - // TalonFX pitchMotor, - TalonFX flywheelMotor, - TalonFX feederMotor + TalonFX shooter1Motor, + TalonFX shooter2Motor, + TalonFX indexerMotor ) { - // m_angleMotor = angleMotor; - // m_pitchMotor = pitchMotor; - m_flywheelMotor = flywheelMotor; - m_feederMotor = feederMotor; - - // Apply the configs - // m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG); - // m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG); - m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG); - m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG); + m_shooter1Motor= shooter1Motor; + m_shooter2Motor= shooter2Motor; + m_indexerMotor = indexerMotor; + + + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG); + m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG); + m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG); } private Angle clampAng(Angle x, Angle min, Angle max){ @@ -45,6 +47,25 @@ public class ShooterReal implements ShooterIO { } } + public static Slot0Configs SHOOTER_PID; + + static { + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = 0.0; + SHOOTER_PID.kP = 0.0; + SHOOTER_PID.kI = 0.0; + SHOOTER_PID.kD = 0.0; + } + + + ConfigurableDouble kV = new ConfigurableDouble("Shooter kV", 0.12); + ConfigurableDouble kP = new ConfigurableDouble("Shooter kP", 0.11); + ConfigurableDouble kI = new ConfigurableDouble("Shooter kI", 0.48); + ConfigurableDouble kD = new ConfigurableDouble("Shooter kD", 0.01); + + + + // // TODO: Test // @Override // public void setShooterAngle(ShooterState state, Angle angle) { @@ -75,37 +96,50 @@ public class ShooterReal implements ShooterIO { // m_pitchMotor.setControl(posRequest); // } + @Override + public void periodic() { + + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = kV.get(); + SHOOTER_PID.kP = kP.get(); + SHOOTER_PID.kI = kI.get(); + SHOOTER_PID.kD = kD.get(); + + m_shooter1Motor.getConfigurator().apply(SHOOTER_PID); + m_shooter2Motor.getConfigurator().apply(SHOOTER_PID); + } + + @Override - public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) { - state.flywheelTargetVelocity = angularVelocity; - // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) - double motorSpeed = angularVelocity.in(RotationsPerSecond) / ShooterConstants.FLYWHEEL_GEAR_RATIO; - VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velocity); + public void setMotor1Velocity(ShooterState state, AngularVelocity target) { + state.motor2TargetVelocity = target; + double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; + m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); + } + + @Override + public void setMotor2Velocity(ShooterState state, AngularVelocity target) { + state.motor2TargetVelocity = target; + double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; + m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); } @Override - public void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) { - state.feederTargetVelocity = linearVelocity; - // (IN / SEC) * (ROT / IN) = (ROT / SEC) - double motorSpeed = linearVelocity.in(InchesPerSecond) / ShooterConstants.FEEDER_INCHES_PER_ROT; - VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velRequest); + public void setIndexerVelocity(ShooterState state, AngularVelocity target) { + state.indexerTargetVelocity = target; + double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; + m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); } @Override public void updateInputs(ShooterState state) { - // state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO); - // state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue(); - // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); - // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + state.motorVelocity = m_shooter1Motor.getVelocity().getValue(); + state.motorVelocity = m_shooter2Motor.getVelocity().getValue(); + state.motorVelocity = m_indexerMotor.getVelocity().getValue(); - state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue(); - state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue(); - - state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); - state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue(); + state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); + state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue(); } }