From: Wesley28w Date: Wed, 11 Feb 2026 22:46:37 +0000 (-0800) Subject: bang bang controller X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=e188bffbe1689c0726a8d2d3fe70ed5c40059039;p=FRC2026.git bang bang controller --- diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9f5e340..97a277b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,16 +1,20 @@ package frc.robot.subsystems.shooter; -import java.lang.annotation.Target; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +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.NeutralModeValue; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,74 +24,122 @@ import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs; 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); - - private TalonFX feederMotor = new TalonFX(IdConstants.FEEDER_ID, Constants.RIO_CAN); + 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 // Goal Velocity / Double theCircumfrence private double shooterTargetSpeed = 0; - private double feederPower = 0; - public double shooterPower = 1.0; + public double shooterPower = 0.5; + //Velocity in rotations per second VelocityVoltage voltageRequest = new VelocityVoltage(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 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 + } + public Shooter(){ TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 0.2; //tune p value + config.Slot0.kP = 676767.0; //tune p value 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; + shooterMotorLeft.getConfigurator().apply(config); shooterMotorRight.getConfigurator().apply(config); - shooterMotorLeft.getConfigurator().apply( + shooterMotorRight.getConfigurator().apply( new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast) ); - shooterMotorRight.getConfigurator().apply( + shooterMotorLeft.getConfigurator().apply( new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) ); - feederMotor.getConfigurator().apply( - new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast) - ); + // set start up for phase initially: + phase = FlywheelPhase.START_UP; SmartDashboard.putData("Turn on shooter", new InstantCommand(() -> setShooter(ShooterConstants.SHOOTER_VELOCITY))); - SmartDashboard.putData("Turn on feeder", new InstantCommand(() -> setFeeder(ShooterConstants.FEEDER_RUN_POWER))); SmartDashboard.putData("Turn ALL off", new InstantCommand(() -> deactivateShooterAndFeeder())); SmartDashboard.putData("Turn off Shooter", new InstantCommand(() -> setShooter(0))); - SmartDashboard.putData("Turn off feeder", new InstantCommand(() -> setFeeder(0))); } public void periodic(){ updateInputs(); + runVelocity(Units.rotationsToRadians(getShooterVelcoity())); SmartDashboard.putNumber("Shot Power", shooterPower); shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower); - shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); - shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); - // shooterMotorLeft.set(-shooterPower); - // shooterMotorRight.set(-shooterPower); - feederMotor.set(feederPower); + 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)); + } + } + + /** Run closed loop at the specified velocity. */ + private void runVelocity(double velocityRadsPerSec) { + // if we are not in the tolerance + 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); + + if (!torqueCurrentControl && lastTorqueCurrentControl) { + launchCount++; + } + lastTorqueCurrentControl = torqueCurrentControl; + + phase = + torqueCurrentControl + ? FlywheelPhase.CONSTANT_TORQUE + : FlywheelPhase.BANG_BANG; + shooterTargetSpeed = Units.radiansToRotations(velocityRadsPerSec); + Logger.recordOutput("Shooter/Setpoint", velocityRadsPerSec); } + public void deactivateShooterAndFeeder() { - setFeeder(0); setShooter(0); System.out.println("Shooter deactivated"); } - public void setFeeder(double power){ - System.out.println("VELOCITY: " + getShooterVelcoity()); - feederPower = power; - } public void setShooter(double velocityRPS) { shooterTargetSpeed = velocityRPS; @@ -98,10 +150,6 @@ public class Shooter extends SubsystemBase implements ShooterIO { this.shooterPower = power; } - public double getFeederVelocity() { - return inputs.feederVelocity; - } - public double getShooterVelcoity() { return inputs.leftShooterVelocity; // assuming they are the same rn } @@ -109,9 +157,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { 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(); + inputs.rightShooterCurrent = shooterMotorRight.getStatorCurrent().getValueAsDouble(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 188e1e4..c49f49e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,10 +1,8 @@ package frc.robot.subsystems.shooter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - public class ShooterConstants { //TODO: find these values - public static final double FEEDER_RUN_POWER = 0.3; // meters per second?? + public static final double FEEDER_RUN_POWER = 0.5; // meters per second?? public static double SHOOTER_VELOCITY = 30.0; // meters per second public static final double SHOOTER_GEAR_RATIO = 36.0 / 24.0; // gear ratio from motors to shooter wheel // public static final double SHOOTER_LAUNCH_DIAMETER = 0.0762; // meters (3 inches) @@ -16,7 +14,10 @@ public class ShooterConstants { // in m/s public static final double EXIT_VELOCITY_TOLERANCE = 1.0; + // for bang bang + public static final double TORQUE_CURRENT_CONTROL_TOLERANCE = 10; // velocity (rotations per second) + public static final double TORQUE_CURRENT_CONTROL_GOAL_AMP = 40; // TUNE } // 8 velcocity is too little // 16 is too much \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 00585ff..36ccc1a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -14,4 +14,4 @@ public interface ShooterIO { } public void updateInputs(); -} +} \ No newline at end of file