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;
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