From 176171f3616cf6dc6187056503d66e932b6e6518 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Sat, 7 Mar 2026 13:33:58 -0500 Subject: [PATCH] Shooter Subsystem --- src/main/java/frc/robot/Constants.java | 18 ++- .../{Intake.java => IntakeSubsystem.java} | 4 +- .../frc/robot/subsystems/TurretSubsystem.java | 148 ++++++++++++++++++ 3 files changed, 167 insertions(+), 3 deletions(-) rename src/main/java/frc/robot/subsystems/{Intake.java => IntakeSubsystem.java} (96%) create mode 100644 src/main/java/frc/robot/subsystems/TurretSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 16168d8..5f89a34 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,10 @@ package frc.robot; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Unit; +import edu.wpi.first.units.measure.Angle; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -65,5 +69,17 @@ public final class Constants { public static final int RETRACT_CHANNEL = 1; public static final int INTAKE_MOTOR_ID = 2; } - + + public static class ShooterConstants { + public static final int LIMIT_CHANNEL = 0; + public static final int TURRET_ID = 1; + public static final int LAUNCHER_ID = 2; + public static final int HOOD_CHANNEL = 3; + public static final int HOOD_ENCODER_ID = 4; + public static final Angle LIMIT_POS = edu.wpi.first.units.Units.Degrees.of(265); + public static final Angle LIMIT_NEG = edu.wpi.first.units.Units.Degrees.of(5); + public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1); + public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90); + public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java similarity index 96% rename from src/main/java/frc/robot/subsystems/Intake.java rename to src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 0ebfee1..ad0381d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -14,9 +14,9 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj2.command.Command; -public class Intake extends TestableSubsystem { +public class IntakeSubsystem extends TestableSubsystem { /** Creates a new ExampleSubsystem. */ - public Intake() + public IntakeSubsystem() { extendRelay = new DigitalOutput(Constants.IntakeConstants.EXTEND_CHANNEL); retractRelay = new DigitalOutput(Constants.IntakeConstants.RETRACT_CHANNEL); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java new file mode 100644 index 0000000..6b7c7c2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -0,0 +1,148 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import org.fairportrobotics.frc.posty.TestableSubsystem; +import org.fairportrobotics.frc.posty.test.PostTest; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; + +import static org.fairportrobotics.frc.posty.assertions.Assertions.*; +import frc.robot.Constants; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj2.command.Command; + +public class TurretSubsystem extends TestableSubsystem { + public enum ShootyRootyTootyState { + INIT, + HOMING, + MANUAL; + } + + private ShootyRootyTootyState currentState; + private DigitalInput limitSwitch; + private TalonFX turret; + private TalonFX launcher; + private Servo hood; + private CANcoder hoodEncoder; + private Angle limitPos; + private Angle limitNeg; + private boolean turretGoingPos; + private Angle targElev; + + public TurretSubsystem() { + limitSwitch = new DigitalInput(Constants.ShooterConstants.LIMIT_CHANNEL); + turret = new TalonFX(Constants.ShooterConstants.TURRET_ID); + launcher = new TalonFX(Constants.ShooterConstants.LAUNCHER_ID); + hood = new Servo(Constants.ShooterConstants.HOOD_CHANNEL); + hoodEncoder = new CANcoder(Constants.ShooterConstants.HOOD_ENCODER_ID); + currentState = ShootyRootyTootyState.INIT; + limitNeg = Constants.ShooterConstants.LIMIT_NEG; + limitPos = Constants.ShooterConstants.LIMIT_POS; + turretGoingPos = false; + targElev = edu.wpi.first.units.Units.Degrees.of(45); + } + + public void setLauncher(double speed) + { + launcher.set(speed); + } + + public void setTargetElevation(Angle elev) + { + if(elev.gt(Constants.ShooterConstants.LIMIT_ELEVATION_NEG) && elev.lt(Constants.ShooterConstants.LIMIT_ELEVATION_POS)) + { + targElev=elev; + } + } + + private static final double homingSpeed=.35; + public void startHoming() + { + turret.set(homingSpeed); + currentState = ShootyRootyTootyState.HOMING; + } + + public void startRotate(double speed) + { + turretGoingPos=speed>0; + + if(!passedLimit()) + { + turret.set(speed); + } + else + { + turret.set(0); + } + } + + public boolean passedLimit() + { + return ((getTurretAngle().gt(limitPos) && turretGoingPos)||(getTurretAngle().lt(limitNeg) && !turretGoingPos)); + } + + private static final double turretGearRatio=1; + public Angle getTurretAngle() + { + return turret.getPosition().refresh().getValue().times(turretGearRatio); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + switch (currentState) { + case HOMING: + periodicHoming(); + break; + case MANUAL: + periodicManual(); + break; + + default: + break; + } + } + + private void periodicHoming() + { + if(limitSwitch.get()) + { + currentState = ShootyRootyTootyState.MANUAL; + turret.set(0); + turret.setPosition(0); + } + } + + private void periodicManual() + { + if(passedLimit()) + { + turret.set(0); + } + + Angle curElev = hoodEncoder.getAbsolutePosition().getValue(); + if(targElev.minus(curElev).abs(Units.Degrees)>Constants.ShooterConstants.TARGET_ELEVATION_DIF.in(Units.Degrees)) + { + if(targElev.gt(curElev)) + { + hood.set(50); + } + else + { + hood.set(-50); + } + } + else + { + hood.set(0); + } + } +}