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;
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;
this.shooterPower = power;
}
- public double getFeederVelocity() {
- return inputs.feederVelocity;
- }
-
public double getShooterVelcoity() {
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();
+ inputs.rightShooterCurrent = shooterMotorRight.getStatorCurrent().getValueAsDouble();
}
-}
+}
\ No newline at end of file