From 14a0023b4764e525a07fe7e9feffd722b72f291d Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Sat, 14 Feb 2026 12:42:16 -0800 Subject: [PATCH] Fixed shooter + Spindexer is now tracking ball, but it is wrong will fix later --- .../frc/robot/subsystems/shooter/Shooter.java | 130 ++++++++---------- .../robot/subsystems/shooter/ShooterIO.java | 8 +- .../robot/subsystems/spindexer/Spindexer.java | 6 +- .../spindexer/SpindexerConstants.java | 5 + 4 files changed, 71 insertions(+), 78 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 1af0998..af372ff 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,121 +1,111 @@ -package frc.robot.subsystems.shooter; - -import static edu.wpi.first.units.Units.MetersPerSecond; +package frc.robot.subsystems.Shooter; 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; -import edu.wpi.first.math.filter.Debouncer.DebounceType; +import au.grapplerobotics.ConfigurationFailedException; +import au.grapplerobotics.LaserCan; +import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; +import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; +import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest; +import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; -import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs; public class Shooter extends SubsystemBase implements ShooterIO { - 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); + 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); + + // Goal Velocity / Double theCircumfrence + private double shooterTargetSpeed = 0; + private double feederPower = 0; - public VelocityVoltage flywheelVelocityVoltage = new VelocityVoltage(0).withSlot(0); - - // Goal Velocity - private double targetSpeedMPS = 0; - private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); + public boolean shooterAtMaxSpeed = false; + public boolean ballDetected = false; + //Velocity in rotations per second + VelocityVoltage voltageRequest = new VelocityVoltage(0); - private double torqueCurrentControlTolerance = 5.0; - private double torqueCurrentDebounceTime = 0.025; + private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - private Debouncer torqueCurrentDebouncer = new Debouncer(torqueCurrentDebounceTime, DebounceType.kFalling); - @AutoLogOutput private long launchCount = 0; - private boolean lastTorqueCurrentControl = false; + double powerModifier; public Shooter(){ + + updateInputs(); + TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 0.15; + config.Slot0.kP = 0.1; //tune p value config.Slot0.kI = 0; - config.Slot0.kD = 0.03; - config.Slot0.kV = 0.20; - - config.MotionMagic = new MotionMagicConfigs().withMotionMagicCruiseVelocity(90).withMotionMagicAcceleration(130); - - motorLeft.getConfigurator().apply(config); - motorRight.getConfigurator().apply(config); - - motorRight.getConfigurator().apply( - new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) + config.Slot0.kD = 0; + config.Slot0.kV = 0.12; //Maximum rps = 100 --> 12V/100rps + shooterMotorLeft.getConfigurator().apply(config); + shooterMotorRight.getConfigurator().apply(config); + + shooterMotorLeft.getConfigurator().apply( + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast) ); - motorLeft.getConfigurator().apply( + shooterMotorRight.getConfigurator().apply( new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) ); - - motorRight.setControl(new Follower(motorLeft.getDeviceID(), MotorAlignmentValue.Opposed)); - - SmartDashboard.putData("Turn on shooter", new InstantCommand(() -> setShooter(ShooterConstants.SHOOTER_VELOCITY))); - SmartDashboard.putData("Turn off Shooter", new InstantCommand(() -> setShooter(0))); } + @Override public void periodic(){ updateInputs(); - Logger.processInputs("Shooter", inputs); - countLaunch(Units.rotationsToRadians(targetSpeedMPS)); - double rps = Units.radiansToRotations(targetSpeedMPS / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2.0)); - motorLeft.setControl(flywheelVelocityVoltage.withVelocity(rps)); + powerModifier = SmartDashboard.getNumber("shooter power modifier", powerModifier); + SmartDashboard.putNumber("shooter power modifier", powerModifier); + shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed * powerModifier)); + shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed * powerModifier)); } - private void countLaunch(double velocityRadsPerSec) { - boolean inTolerance = - Math.abs(Units.rotationsToRadians(getShooterVelcoity()) - velocityRadsPerSec) - <= torqueCurrentControlTolerance; - - boolean torqueCurrentControl = torqueCurrentDebouncer.calculate(inTolerance); - - if (!torqueCurrentControl && lastTorqueCurrentControl) { - launchCount++; - } - lastTorqueCurrentControl = torqueCurrentControl; + public void deactivateShooterAndFeeder() { + setFeeder(0); + setShooter(0); + } - Logger.recordOutput("Shooter/Setpoint", velocityRadsPerSec); + public void setFeeder(double power){ + // System.out.println("VELOCITY: " + getShooterVelcoity()); + feederPower = power; } - public void deactivateShooterAndFeeder() { - setShooter(0); + public void setShooter(double linearVelocityMps) { + shooterTargetSpeed = linearVelocityMps; } - public void setShooter(double velocityMPS) { - targetSpeedMPS = velocityMPS; + /**@return velocity in m/s */ + public double getShooterVelcoity(){ + return inputs.shooterSpeedLeft; } - public double getShooterVelcoity() { - return inputs.leftShooterVelocity; + @Override + public void updateInputs(){ + inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()); + inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble()); + Logger.processInputs("Shooter", inputs); } - public boolean atTargetVelocity(){ - return Math.abs(getShooterVelcoity() - targetSpeedMPS) < 0.1; // Tolerance of 0.1 mps + public boolean atTargetSpeed(){ + return Math.abs(getShooterVelcoity() - shooterTargetSpeed) < 1.0; } - public void updateInputs() { - inputs.leftShooterVelocity = Units.rotationsToRadians(motorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; - inputs.rightShooterVelocity =Units.rotationsToRadians(motorRight.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; - inputs.leftShooterCurrent = motorLeft.getStatorCurrent().getValueAsDouble(); - inputs.rightShooterCurrent = motorRight.getStatorCurrent().getValueAsDouble(); + @AutoLogOutput(key="Shooter/TargetSpeed") + public double getTargetVelocity(){ + return Units.rotationsToRadians(shooterTargetSpeed); } } \ 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..872b136 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -5,12 +5,8 @@ import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { @AutoLog public static class ShooterIOInputs { - public double rightShooterVelocity = 0; - public double leftShooterVelocity = 0; - public double feederVelocity = 0; - public double rightShooterCurrent = 0; - public double leftShooterCurrent = 0; - public double feederCurrent = 0; + public double shooterSpeedLeft = 0.0; + public double shooterSpeedRight = 0.0; } public void updateInputs(); diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 0ba8e4d..a0106a4 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -12,7 +12,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO{ TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID); private double power = 0.0; - + public int ballCount = 0; private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged(); public Spindexer(){ @@ -23,8 +23,10 @@ public class Spindexer extends SubsystemBase implements SpindexerIO{ public void periodic() { power = SmartDashboard.getNumber("Spindexer Power", power); SmartDashboard.putNumber("Spindexer Power", power); - motor.set(power); + if (inputs.spindexerVelocity < SpindexerConstants.spindexerVelocityWithBall) { + ballCount++; + } } public void maxSpindexer(){ diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java new file mode 100644 index 0000000..a48f7c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.spindexer; + +public class SpindexerConstants { + public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls) +} -- 2.39.5