From a68324c64b1bdc21808d33f50ff1271b5c9883ab Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Sat, 17 Jan 2026 16:31:02 -0800 Subject: [PATCH] Shooter --- src/main/java/frc/robot/RobotContainer.java | 3 + .../java/frc/robot/constants/Constants.java | 1 + .../java/frc/robot/constants/IdConstants.java | 6 ++ .../frc/robot/subsystems/shooter/Shooter.java | 102 ++++++++++++++++++ .../subsystems/shooter/ShooterConstants.java | 18 ++++ 5 files changed, 130 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter/Shooter.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 52c3f12..6598404 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.GyroIOPigeon2; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.turret.Turret; import frc.robot.util.PathGroupLoader; import frc.robot.util.Vision.DetectedObject; @@ -43,6 +44,7 @@ public class RobotContainer { private Drivetrain drive = null; private Vision vision = null; private Turret turret = null; + private Shooter shooter = null; private Command auto = new DoNothing(); // Controllers are defined here @@ -56,6 +58,7 @@ public class RobotContainer { */ public RobotContainer(RobotId robotId) { turret = new Turret(); + shooter = new Shooter(); // dispatch on the robot switch (robotId) { case TestBed1: diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 6b99d88..0a519e0 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -12,6 +12,7 @@ public class Constants { // CAN bus names public static final String CANIVORE_CAN = "CANivore"; + public static final String SUBSYSTEM_CANIVORE_CAN = "CANivoreSub"; public static final String RIO_CAN = "rio"; // Logging diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index 9ae589e..7971bdd 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -21,4 +21,10 @@ public class IdConstants { // Motor ID public static final int TURRET_MOTOR_ID = 20; + + // Shooter + public static final int SHOOTER_LEFT_ID = 6; + public static final int SHOOTER_RIGHT_ID = 4; + public static final int FEEDER_ID = 3; + public static final int SHOOTER_SENSOR_ID = 1; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..e69563e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,102 @@ +package frc.robot.subsystems.shooter; + +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.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +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.units.Measure; +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.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; + +public class Shooter { + + 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); + + private TalonFX feederMotor = new TalonFX(IdConstants.FEEDER_ID, Constants.SUBSYSTEM_CANIVORE_CAN); + + //rotations/sec + + // Goal Velocity / Double theCircumfrence + private double shooterTargetSpeed = 0; + private double feederPower = 0; + //Velocity in rotations per second + VelocityVoltage voltageRequest = new VelocityVoltage(0); + + public Shooter(){ + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = 0.1; //tune p value + config.Slot0.kI = 0; + 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) + ); + + shooterMotorRight.getConfigurator().apply( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) + ); + + feederMotor.getConfigurator().apply( + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast) + ); + + 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(){ + shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); + shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); + feederMotor.set(feederPower); + } + + 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 linearVelocityMps) { + double wheelCircumference = Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER; + shooterTargetSpeed = linearVelocityMps * ShooterConstants.SHOOTER_GEAR_RATIO / wheelCircumference; // rps + } + + public double getFeederVelocity() { + return feederMotor.getVelocity().getValueAsDouble(); + } + public double getShooterVelcoity() { + return shooterMotorLeft.getVelocity().getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java new file mode 100644 index 0000000..c5fef88 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.shooter; + +public class ShooterConstants { + //TODO: find these values + public static final double FEEDER_RUN_POWER = 0.3; // meters per second?? + public static final double SHOOTER_VELOCITY = 10.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) + public static final double SHOOTER_LAUNCH_DIAMETER = 0.1016; // meters (4 inches) I think this is right. + public static final double SHOOTER_RUN_POWER = 0.3; + public static final double SENSOR_DISTANNCE_THRESHOLD = 0.150; // meters + public static final double SENSOR_AMBIENCE_THRESHOLD = 0.100; // meters + + // in m/s + public static final double EXIT_VELOCITY_TOLERANCE = 1.0; +} +// 8 velcocity is too little +// 16 is too much \ No newline at end of file -- 2.39.5