From: mixxlto Date: Thu, 12 Feb 2026 07:35:11 +0000 (-0800) Subject: Update Shooter.java X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=cf4c6f63364f78ee6e0374529af1093dc0851542;p=FRC2026.git Update Shooter.java --- diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 1317fa3..0299e4d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,15 +1,20 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.MetersPerSecond; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.controls.Follower; // Added for following import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.filter.Debouncer; @@ -24,139 +29,89 @@ import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs; public class Shooter extends SubsystemBase implements ShooterIO { - private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); - private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); - - //rotations/sec + private TalonFX motorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); + private TalonFX motorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); - // Goal Velocity / Double theCircumfrence - private double shooterTargetSpeed = 0; - - public double shooterPower = 0.5; - //Velocity in rotations per second - VelocityVoltage voltageRequest = new VelocityVoltage(0); + public VelocityVoltage flywheelVelocityVoltage = new VelocityVoltage(0).withSlot(0); + + // Goal Velocity + private double targetSpeedMPS = 0; private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - // for tracking current phase: used to adjust the setting - private FlywheelPhase phase; - - private double torqueCurrentControlTolerance = 20.0; + private double torqueCurrentControlTolerance = 5.0; private double torqueCurrentDebounceTime = 0.025; - private double atGoalStateDebounceTime = 0.2; - /* - * Debouncers take in certain statement. If it is false: it checks for how long (the first parameter) - * If it is long enough then return True. - */ private Debouncer torqueCurrentDebouncer = new Debouncer(torqueCurrentDebounceTime, DebounceType.kFalling); - private Debouncer atGoalStateDebouncer = new Debouncer(atGoalStateDebounceTime, DebounceType.kFalling); - private boolean atGoal = false; @AutoLogOutput private long launchCount = 0; - private boolean lastTorqueCurrentControl = false; - - public enum FlywheelPhase { - BANG_BANG, - CONSTANT_TORQUE, - START_UP - } + private boolean lastTorqueCurrentControl = false; public Shooter(){ TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 676767.0; //tune p value + config.Slot0.kP = 0.15; config.Slot0.kI = 0; - config.Slot0.kD = 0; - config.Slot0.kV = 0.12; //Maximum rps = 100 --> 12V/100rps - - config.TorqueCurrent.PeakForwardTorqueCurrent = 40; // this is the constant torque velocity - config.TorqueCurrent.PeakReverseTorqueCurrent = 0; // we are making this a BANG BANG controller for talon fx - config.MotorOutput.PeakForwardDutyCycle = 1.0; - config.MotorOutput.PeakReverseDutyCycle = 0.0; + config.Slot0.kD = 0.03; + config.Slot0.kV = 0.20; + + config.MotionMagic = new MotionMagicConfigs().withMotionMagicCruiseVelocity(90).withMotionMagicAcceleration(130); - shooterMotorLeft.getConfigurator().apply(config); - shooterMotorRight.getConfigurator().apply(config); + motorLeft.getConfigurator().apply(config); + motorRight.getConfigurator().apply(config); - shooterMotorRight.getConfigurator().apply( - new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast) - ); + motorRight.setControl(new Follower(motorLeft.getDeviceID(), MotorAlignmentValue.Opposed)); - shooterMotorLeft.getConfigurator().apply( + motorRight.getConfigurator().apply( new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) ); - // set start up for phase initially: - phase = FlywheelPhase.START_UP; + motorLeft.getConfigurator().apply( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) + ); SmartDashboard.putData("Turn on shooter", new InstantCommand(() -> setShooter(ShooterConstants.SHOOTER_VELOCITY))); - SmartDashboard.putData("Turn ALL off", new InstantCommand(() -> deactivateShooterAndFeeder())); SmartDashboard.putData("Turn off Shooter", new InstantCommand(() -> setShooter(0))); } public void periodic(){ updateInputs(); - runVelocity(Units.rotationsToRadians(getShooterVelcoity())); - SmartDashboard.putNumber("Shot Power", shooterPower); - shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower); - - if (phase == FlywheelPhase.BANG_BANG || phase == FlywheelPhase.START_UP) { // shooter target speed is in RPS - // Duty-cycle bang-bang (apply to both) 100% speeds - shooterMotorLeft.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // spin as fast as possible shooter target speed - shooterMotorRight.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // same here - } else { - // Torque-current bang-bang - shooterMotorLeft.setControl(new TorqueCurrentFOC(ShooterConstants.TORQUE_CURRENT_CONTROL_GOAL_AMP)); - shooterMotorRight.setControl(new TorqueCurrentFOC(ShooterConstants.TORQUE_CURRENT_CONTROL_GOAL_AMP)); - } + Logger.processInputs("Shooter", inputs); + countLaunch(Units.rotationsToRadians(targetSpeedMPS)); + + double rps = Units.radiansToRotations(targetSpeedMPS / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2.0)); + motorLeft.setControl(flywheelVelocityVoltage.withVelocity(rps)); } - /** Run closed loop at the specified velocity. */ - private void runVelocity(double velocityRadsPerSec) { - // if we are not in the tolerance + private void countLaunch(double velocityRadsPerSec) { boolean inTolerance = Math.abs(Units.rotationsToRadians(getShooterVelcoity()) - velocityRadsPerSec) <= torqueCurrentControlTolerance; - // If we are not in tolerance we calculate for how long - boolean torqueCurrentControl = torqueCurrentDebouncer.calculate(inTolerance); // this calculates if the logic has been occuring long enough - atGoal = atGoalStateDebouncer.calculate(inTolerance); + boolean torqueCurrentControl = torqueCurrentDebouncer.calculate(inTolerance); if (!torqueCurrentControl && lastTorqueCurrentControl) { - launchCount++; + launchCount++; } lastTorqueCurrentControl = torqueCurrentControl; - phase = - torqueCurrentControl - ? FlywheelPhase.CONSTANT_TORQUE - : FlywheelPhase.BANG_BANG; - shooterTargetSpeed = Units.radiansToRotations(velocityRadsPerSec); Logger.recordOutput("Shooter/Setpoint", velocityRadsPerSec); } - public void deactivateShooterAndFeeder() { setShooter(0); - System.out.println("Shooter deactivated"); - } - - public void setShooter(double velocityRPS) { - shooterTargetSpeed = velocityRPS; - System.out.println("Shooter is working"); } - public void setShooterPower(double power){ - this.shooterPower = power; + public void setShooter(double velocityMPS) { + targetSpeedMPS = velocityMPS; } public double getShooterVelcoity() { - return inputs.leftShooterVelocity; // assuming they are the same rn + return inputs.leftShooterVelocity; } public void updateInputs() { - inputs.leftShooterVelocity = shooterMotorLeft.getVelocity().getValueAsDouble(); - inputs.rightShooterVelocity = shooterMotorRight.getVelocity().getValueAsDouble(); - inputs.leftShooterCurrent = shooterMotorLeft.getStatorCurrent().getValueAsDouble(); - inputs.rightShooterCurrent = shooterMotorRight.getStatorCurrent().getValueAsDouble(); + inputs.leftShooterVelocity = motorLeft.getVelocity().getValueAsDouble(); + inputs.rightShooterVelocity = motorRight.getVelocity().getValueAsDouble(); + inputs.leftShooterCurrent = motorLeft.getStatorCurrent().getValueAsDouble(); + inputs.rightShooterCurrent = motorRight.getStatorCurrent().getValueAsDouble(); } } \ No newline at end of file