From 431be205e17b5f2d90f0cc1b7fb1b6ecc18333fe Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 27 Mar 2026 23:23:39 -0400 Subject: [PATCH 01/27] Turret tracking from field location --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 5 +- .../AutoShootCommandChassisTurret.java | 2 +- .../frc/robot/commands/AutoTurretCommand.java | 87 +++++++++++++++++++ .../frc/robot/commands/ExampleCommand.java | 43 +++++++++ .../robot/commands/ManualShootCommand.java | 6 +- .../frc/robot/subsystems/TurretSubsystem.java | 51 +++++------ 7 files changed, 161 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AutoTurretCommand.java create mode 100644 src/main/java/frc/robot/commands/ExampleCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 62a15ef..dfcf13b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -99,7 +99,7 @@ public final class Constants { public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165); public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); public static final double HOMING_SPEED = .35; - public static final double TURRET_GEAR_RATIO = 1; + public static final double TURRET_GEAR_RATIO = 6; public static final int HOOD_SERVO_CHANNEL = 0; public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 397d7ad..54fa268 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.AutoShootCommandChassisTurret; +import frc.robot.commands.AutoTurretCommand; import frc.robot.commands.ManualHopperCommand; import frc.robot.commands.ManualIntake; import frc.robot.commands.ManualShootCommand; @@ -42,6 +43,9 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); + // Configure the trigger bindings configureBindings(); } @@ -57,7 +61,6 @@ public class RobotContainer { */ private void configureBindings() { - turretSubsystem.startHoming(); m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index fc9a56b..f28a7b0 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -105,7 +105,7 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlignState", "Look for tag"); Pose3d botPose = driveSubsystem.getBotPose(); - driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this + driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); } else // We have an april tag, center it to the camera frame { diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java new file mode 100644 index 0000000..c30401b --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -0,0 +1,87 @@ +package frc.robot.commands; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.TurretSubsystem; + +public class AutoTurretCommand extends Command { + + private final Translation2d CENTER_TURRET_TO_ROBOT = new Translation2d(-0.1335024, -0.1824736); + + private TurretSubsystem turretSubsystem; + private DriveSubsystem driveSubsystem; + + private int[] shootingTagFilters; + + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + + private Pose3d closestTag; + private Pose3d scoringHubPose; + + public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) { + this.turretSubsystem = turretSubsystem; + this.driveSubsystem = driveSubsystem; + + addRequirements(turretSubsystem); // Not adding drive as we don't wan't the drive command to be de-scheduled + } + + @Override + public void initialize() { + // Perform auto-home + + DriverStation.getAlliance().ifPresent((aliance) -> { + if (aliance == Alliance.Blue) { + shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; + + scoringHubPose = fieldTags.getTagPose(26).get(); + scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(0.5207, 0, Rotation2d.kZero))); + } else { + shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; + + scoringHubPose = fieldTags.getTagPose(10).get(); + scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + } + Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose); + }); + + } + + @Override + public void execute() { + + Pose3d botPose = driveSubsystem.getBotPose(); + Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), + new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngle().in(Units.Rotations))))); + Logger.recordOutput("AutoTurret-TurretPose", turretPose); + + Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); + + turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).getMeasure()); + + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + } + +} diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java new file mode 100644 index 0000000..7481d3c --- /dev/null +++ b/src/main/java/frc/robot/commands/ExampleCommand.java @@ -0,0 +1,43 @@ +// 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.commands; + +import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class ExampleCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ExampleSubsystem m_subsystem; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ExampleCommand(ExampleSubsystem subsystem) { + m_subsystem = subsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ManualShootCommand.java b/src/main/java/frc/robot/commands/ManualShootCommand.java index dbe750c..cb397b0 100644 --- a/src/main/java/frc/robot/commands/ManualShootCommand.java +++ b/src/main/java/frc/robot/commands/ManualShootCommand.java @@ -26,17 +26,17 @@ public class ManualShootCommand extends Command { public void execute() { if(m_controller.getRightTriggerAxis()>.5 && m_controller.getLeftTriggerAxis()==0) { - m_subsystem.startRotate(-.5); + m_subsystem.turretRotateDir(-.5); m_controller.setRumble(RumbleType.kRightRumble, 1); } else if(m_controller.getLeftTriggerAxis()>.5 && m_controller.getRightTriggerAxis()==0) { - m_subsystem.startRotate(.5); + m_subsystem.turretRotateDir(.5); m_controller.setRumble(RumbleType.kLeftRumble, 1); } else { - m_subsystem.startRotate(0); + m_subsystem.turretRotateDir(0); m_controller.setRumble(RumbleType.kBothRumble, 0); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 87af396..a172c06 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.sim.TalonFXSimState; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; @@ -43,6 +44,7 @@ public class TurretSubsystem extends TestableSubsystem { private TurretState currentState; private DigitalInput turretLimitSwitch; private TalonFX turretMotor; + private TalonFXSimState turretSim; private SparkMax launcherMotor; private SparkClosedLoopController launcherControler; @@ -73,6 +75,8 @@ public class TurretSubsystem extends TestableSubsystem { ) ); + turretSim = turretMotor.getSimState(); + // Launcher configuration launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless); SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); @@ -119,7 +123,7 @@ public class TurretSubsystem extends TestableSubsystem { currentState = TurretState.HOMING; } - public void startRotate(double speed) + public void turretRotateDir(double speed) { turretGoingPos=speed>0; @@ -139,23 +143,24 @@ public class TurretSubsystem extends TestableSubsystem { public Angle getTurretAngle() { - return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO); + return turretMotor.getPosition().refresh().getValue().div(Constants.ShooterConstants.TURRET_GEAR_RATIO); } + @Override public void periodic() { // This method will be called once per scheduler run - switch (currentState) { - case HOMING: - periodicHoming(); - break; - case MANUAL: - periodicManual(); - break; + // switch (currentState) { + // case HOMING: + // periodicHoming(); + // break; + // case MANUAL: + // periodicManual(); + // break; - default: - break; - } + // default: + // break; + // } if(LOGGING) { @@ -171,26 +176,14 @@ public class TurretSubsystem extends TestableSubsystem { currentState = TurretState.MANUAL; turretMotor.set(0); turretMotor.setPosition(0); - goTo(limitPos); + setTurretRotation(limitPos); } } - private void goTo(Angle pos) + public void setTurretRotation(Angle pos) { - if(pos.gt(limitNeg) && pos.lt(limitPos)) - { - while(getTurretAngle()!=pos) - { - if(getTurretAngle().gt(pos)) - { - turretMotor.set(.5); - } - else - { - turretMotor.set(-.5); - } - } - } + turretMotor.setControl(new PositionVoltage(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO))); + turretSim.setRawRotorPosition(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); } private void periodicManual() @@ -203,7 +196,7 @@ public class TurretSubsystem extends TestableSubsystem { @PostTest() public void TurretSubsystem_CANDevicesConnected(){ - assertThat(launcherMotor.getDeviceId()).as("Launcher motor not connected!").isGreaterThan(0); + assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isTrue(); assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue(); } From bf3e87ea06387c0504207ed9cec8ccf75bfe135f Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sat, 28 Mar 2026 17:01:16 -0400 Subject: [PATCH 02/27] Attempting to tune turret --- src/main/java/frc/robot/RobotContainer.java | 1 + .../AutoShootCommandChassisTurret.java | 5 +- .../frc/robot/commands/AutoTurretCommand.java | 11 +- .../frc/robot/subsystems/DriveSubsystem.java | 2 +- .../frc/robot/subsystems/TurretSubsystem.java | 133 ++++++++++-------- 5 files changed, 84 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 54fa268..3cbede4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,6 +45,7 @@ public class RobotContainer { public RobotContainer() { turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); + // turretSubsystem.homeTurret(); // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index f28a7b0..1395384 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -40,7 +40,7 @@ public class AutoShootCommandChassisTurret extends Command{ private int[] tagFilters; - private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); private Pose3d closestTag; @@ -91,7 +91,8 @@ public class AutoShootCommandChassisTurret extends Command{ Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); - deadreckoningAutoCenterController.setSetpoint(delta.getAngle().getRadians()); + // Add 180 because we want the back of the robot to face the HUB + deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); } @Override diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index c30401b..11929aa 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -28,7 +29,7 @@ public class AutoTurretCommand extends Command { private int[] shootingTagFilters; - private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); private Pose3d closestTag; private Pose3d scoringHubPose; @@ -43,13 +44,14 @@ public class AutoTurretCommand extends Command { @Override public void initialize() { // Perform auto-home + turretSubsystem.homeTurret(); DriverStation.getAlliance().ifPresent((aliance) -> { if (aliance == Alliance.Blue) { shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; scoringHubPose = fieldTags.getTagPose(26).get(); - scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(0.5207, 0, Rotation2d.kZero))); + scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); } else { shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; @@ -71,7 +73,10 @@ public class AutoTurretCommand extends Command { Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); - turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).getMeasure()); + if(turretSubsystem.isTurretReady()){ + turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).getMeasure()); + // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(45, Units.Degrees)); + } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 485a5f0..6dbad45 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -135,7 +135,7 @@ public class DriveSubsystem extends TestableSubsystem { // Set vision measurement confidence values - driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999)); + driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7)); this.setDefaultCommand(Commands.run(new Runnable() { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index a172c06..4ab6c65 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -12,8 +12,10 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -28,6 +30,8 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.Constants; +import edu.wpi.first.units.AngleUnit; +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; @@ -37,21 +41,21 @@ public class TurretSubsystem extends TestableSubsystem { public enum TurretState { INIT, HOMING, - MANUAL; + READY; } private final boolean LOGGING = true; - private TurretState currentState; + private TurretState turretState; private DigitalInput turretLimitSwitch; private TalonFX turretMotor; + private PositionVoltage turretControlMode = new PositionVoltage(0); private TalonFXSimState turretSim; private SparkMax launcherMotor; private SparkClosedLoopController launcherControler; + private Angle turretOffset = Angle.ofBaseUnits(0, Units.Degrees); + private Servo hood; - private Angle limitPos; - private Angle limitNeg; - private boolean turretGoingPos; public TurretSubsystem() { @@ -62,16 +66,29 @@ public class TurretSubsystem extends TestableSubsystem { new TalonFXConfiguration() .withSlot0( new Slot0Configs() - .withKP(1.0) - .withKI(0) + .withKP(2) + .withKI(1) .withKD(0) + .withKS(0) ) .withMotorOutput( new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) + .withInverted(InvertedValue.CounterClockwise_Positive) ) .withCurrentLimits( new CurrentLimitsConfigs() + .withStatorCurrentLimit(50) + ) + .withSoftwareLimitSwitch( + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(Angle.ofRelativeUnits(90, Units.Degree).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(Angle.ofRelativeUnits(-90, Units.Degree).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)) + ) + .withFeedback( + new FeedbackConfigs() + .withFeedbackRotorOffset(0) ) ); @@ -88,10 +105,7 @@ public class TurretSubsystem extends TestableSubsystem { // Hood configuration hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL); - currentState = TurretState.INIT; - limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG; - limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS; - turretGoingPos = false; + turretState = TurretState.INIT; } public void setLauncher(double speed) {launcherControler.setSetpoint(speed, ControlType.kVelocity);} @@ -117,81 +131,76 @@ public class TurretSubsystem extends TestableSubsystem { return this.runOnce(() -> setLauncher(-.5)); } - public void startHoming() - { - turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); - currentState = TurretState.HOMING; + public void homeTurret(){ + turretState = TurretState.HOMING; + } + + public boolean isTurretReady(){ + return turretState == TurretState.READY; } public void turretRotateDir(double speed) { - turretGoingPos=speed>0; - - if(!passedLimit()) - { - turretMotor.set(speed); - } - else - { - turretMotor.set(0); - } - } - - public boolean passedLimit(){ - return (getTurretAngle().gt(limitPos) && turretGoingPos) || (getTurretAngle().lt(limitNeg) && !turretGoingPos); + turretMotor.set(speed); } public Angle getTurretAngle() { - return turretMotor.getPosition().refresh().getValue().div(Constants.ShooterConstants.TURRET_GEAR_RATIO); + return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(Angle.ofRelativeUnits(180, Units.Degrees)).times(-1); } + private Angle getTurretMotorPosition(){ + return turretMotor.getPosition().refresh().getValue().minus(turretOffset); + } @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; - // } + if(turretState == TurretState.HOMING) + { + turretMotor.set(-0.05); + + if(turretLimitSwitch.get()) + { // Turret has tripped the switch + turretMotor.stopMotor(); + turretOffset = turretMotor.getPosition().refresh().getValue().plus(Angle.ofRelativeUnits(85, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); + turretState = TurretState.READY; + setTurretRotation(Angle.ofRelativeUnits(0, Units.Degrees)); + } + } if(LOGGING) { - Logger.recordOutput("Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); + Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get()); + Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); + Logger.recordOutput("TurretSubsystem-TurretState", turretState); + Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretMotorPosition", getTurretMotorPosition().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretRequestedPos", turretControlMode.getPositionMeasure().in(Units.Degrees)); } } - private void periodicHoming() - { - if(turretLimitSwitch.get()) - { - currentState = TurretState.MANUAL; - turretMotor.set(0); - turretMotor.setPosition(0); - setTurretRotation(limitPos); - } - } - public void setTurretRotation(Angle pos) { - turretMotor.setControl(new PositionVoltage(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO))); - turretSim.setRawRotorPosition(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); + if(turretState != TurretState.READY) + { + // Not ready to set angle + return; + } + + if(pos.gt(Angle.ofRelativeUnits(90, Units.Degrees)) || pos.lt(Angle.ofRelativeUnits(-90, Units.Degrees))) + { + // Don't allow to over extend + return; + } + + setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); } - private void periodicManual() - { - if(passedLimit()) - { - turretMotor.set(0); - } + public void setTurretMotorRotation(Angle pos){ + turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset))); + turretSim.setRawRotorPosition(pos); } @PostTest() From 2610f4fd776db6d6fb8c195a02b999f9d263c5a1 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sun, 29 Mar 2026 11:34:22 -0400 Subject: [PATCH 03/27] Turret Field orientation --- .../frc/robot/commands/AutoTurretCommand.java | 8 ++- .../frc/robot/subsystems/TurretSubsystem.java | 56 +++++++++++-------- 2 files changed, 39 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index 11929aa..da975ef 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -72,10 +72,12 @@ public class AutoTurretCommand extends Command { Logger.recordOutput("AutoTurret-TurretPose", turretPose); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); - + Logger.recordOutput("AutoTurret-Translation", targetTranslation); + + if(turretSubsystem.isTurretReady()){ - turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).getMeasure()); - // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(45, Units.Degrees)); + turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure()); + // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees)); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 4ab6c65..e477aa0 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -11,6 +11,7 @@ import org.littletonrobotics.junction.Logger; import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -18,7 +19,9 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.ForwardLimitValue; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.ReverseLimitValue; import com.ctre.phoenix6.sim.TalonFXSimState; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; @@ -50,6 +53,9 @@ public class TurretSubsystem extends TestableSubsystem { private TalonFX turretMotor; private PositionVoltage turretControlMode = new PositionVoltage(0); private TalonFXSimState turretSim; + private StatusSignal turretMotorPosition; + private StatusSignal turretMotorForwardLimit; + private StatusSignal turretMotorReverseLimit; private SparkMax launcherMotor; private SparkClosedLoopController launcherControler; @@ -73,24 +79,21 @@ public class TurretSubsystem extends TestableSubsystem { ) .withMotorOutput( new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) + .withInverted(InvertedValue.Clockwise_Positive) ) .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(50) ) - .withSoftwareLimitSwitch( - new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withForwardSoftLimitThreshold(Angle.ofRelativeUnits(90, Units.Degree).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)) - .withReverseSoftLimitEnable(true) - .withReverseSoftLimitThreshold(Angle.ofRelativeUnits(-90, Units.Degree).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)) - ) + .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(false).withReverseSoftLimitEnable(false)) .withFeedback( new FeedbackConfigs() .withFeedbackRotorOffset(0) ) ); + turretMotorPosition = turretMotor.getPosition(); + turretMotorForwardLimit = turretMotor.getForwardLimit(); + turretMotorReverseLimit = turretMotor.getReverseLimit(); turretSim = turretMotor.getSimState(); @@ -146,11 +149,11 @@ public class TurretSubsystem extends TestableSubsystem { public Angle getTurretAngle() { - return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(Angle.ofRelativeUnits(180, Units.Degrees)).times(-1); + return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO); } private Angle getTurretMotorPosition(){ - return turretMotor.getPosition().refresh().getValue().minus(turretOffset); + return turretMotorPosition.refresh().getValue().minus(turretOffset); } @Override @@ -158,14 +161,25 @@ public class TurretSubsystem extends TestableSubsystem { if(turretState == TurretState.HOMING) { - turretMotor.set(-0.05); + turretMotor.set(0.05); if(turretLimitSwitch.get()) { // Turret has tripped the switch turretMotor.stopMotor(); - turretOffset = turretMotor.getPosition().refresh().getValue().plus(Angle.ofRelativeUnits(85, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); + turretOffset = turretMotorPosition.refresh().getValue().plus(Angle.ofRelativeUnits(100, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); turretState = TurretState.READY; - setTurretRotation(Angle.ofRelativeUnits(0, Units.Degrees)); + Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset); + Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset); + Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit); + Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit); + turretMotor.getConfigurator().apply( + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(forwardLimit) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(reverseLimit) + ); + // setTurretRotation(Angle.ofRelativeUnits(0, Units.Degrees)); } } @@ -175,8 +189,12 @@ public class TurretSubsystem extends TestableSubsystem { Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); Logger.recordOutput("TurretSubsystem-TurretState", turretState); Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().in(Units.Degrees)); - Logger.recordOutput("TurretSubsystem-TurretMotorPosition", getTurretMotorPosition().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretRequestedPos", turretControlMode.getPositionMeasure().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffset.in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); + Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue()); } } @@ -189,23 +207,17 @@ public class TurretSubsystem extends TestableSubsystem { return; } - if(pos.gt(Angle.ofRelativeUnits(90, Units.Degrees)) || pos.lt(Angle.ofRelativeUnits(-90, Units.Degrees))) - { - // Don't allow to over extend - return; - } - setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); } public void setTurretMotorRotation(Angle pos){ turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset))); - turretSim.setRawRotorPosition(pos); + turretSim.setRawRotorPosition(pos.plus(turretOffset)); } @PostTest() public void TurretSubsystem_CANDevicesConnected(){ - assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isTrue(); + assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse(); assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue(); } From a06b44edf7cea9c68b1988ed70b14510267f2475 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sun, 29 Mar 2026 14:30:48 -0400 Subject: [PATCH 04/27] Shooting is working --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 5 ++++ .../AutoShootCommandChassisTurret.java | 2 +- .../frc/robot/subsystems/TurretSubsystem.java | 30 +++++++++++++++---- 4 files changed, 32 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0737efd..9d9365f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -117,7 +117,7 @@ public final class Constants { public static class HopperConstants { public static final int KICKER_MOTOR_ID = 9; - public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; public static final int SPINDEXER_MOTOR_ID = 8; public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a2646d..e0bd403 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -70,6 +70,11 @@ public class RobotContainer { // m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); // m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); + + + m_driverController.b().whileTrue(turretSubsystem.setShooter(1500)); + m_driverController.x().whileTrue(turretSubsystem.setShooter(2000)); + m_driverController.y().whileTrue(turretSubsystem.setShooter(3000)); m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index fc9a56b..d30d6fc 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -74,7 +74,7 @@ public class AutoShootCommandChassisTurret extends Command{ } }); - turretSubsystem.setLauncher(2500); + turretSubsystem.setLauncher(5500); hopperSubsystem.spindexerOn(); cameraAutoCenterController.calculate(-100); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6198ada..fae68ca 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -20,6 +20,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; @@ -33,6 +34,7 @@ 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; +import edu.wpi.first.wpilibj2.command.Commands; public class TurretSubsystem extends TestableSubsystem { public enum TurretState { @@ -81,8 +83,14 @@ public class TurretSubsystem extends TestableSubsystem { SparkMaxConfig config = new SparkMaxConfig() .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); - config.closedLoop.p(1).i(0).d(0.1); - config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); + config.voltageCompensation(10); + config.closedLoop.p(0.0001).i(0).d(0.0); // I = 0.0000001 + config.closedLoop.feedForward.kS(0.2).kV(0.002).kA(0.00); + // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + config.closedLoop.positionWrappingEnabled(false); + // config.closedLoopRampRate(0.5); + // config.smartCurrentLimit(5, 50); launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); launcherControler = launcherMotor.getClosedLoopController(); @@ -94,10 +102,13 @@ public class TurretSubsystem extends TestableSubsystem { turretGoingPos = false; } - public void setLauncher(double speed) {launcherControler.setSetpoint(speed, ControlType.kVelocity);} + public void setLauncher(double speed) { + launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + // launcherMotor.set(speed/1000); + } public boolean isLauncherUpToSpeed() { - return launcherControler.isAtSetpoint(); + return true; } public void turretControl(double position) @@ -117,6 +128,14 @@ public class TurretSubsystem extends TestableSubsystem { return this.runOnce(() -> setLauncher(-.5)); } + public Command setShooter(int speedRPM){ + return Commands.runEnd(() -> { + setLauncher(speedRPM); + }, () -> { + launcherMotor.stopMotor(); + }, this); + } + public void startHoming() { //turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); @@ -164,7 +183,8 @@ public class TurretSubsystem extends TestableSubsystem { if(LOGGING) { - Logger.recordOutput("Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); + Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); + Logger.recordOutput("TurretSubsystem-Launcher Volatge", launcherMotor.getOutputCurrent()); } } From 47073cfd352fd117f98a127ed79ccefa550008d1 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sun, 29 Mar 2026 15:00:17 -0400 Subject: [PATCH 05/27] Fixed some issues --- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/commands/AutoShootCommandChassisTurret.java | 4 ++-- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/HopperSubsystem.java | 4 ++-- 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5bc5953..2e5bbb6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -57,7 +57,6 @@ public class RobotContainer { */ private void configureBindings() { - turretSubsystem.startHoming(); m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index d30d6fc..5ee7c8c 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -91,7 +91,7 @@ public class AutoShootCommandChassisTurret extends Command{ Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); - deadreckoningAutoCenterController.setSetpoint(delta.getAngle().getRadians()); + deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); } @Override @@ -105,7 +105,7 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlignState", "Look for tag"); Pose3d botPose = driveSubsystem.getBotPose(); - driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this + //driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this } else // We have an april tag, center it to the camera frame { diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 485a5f0..6dbad45 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -135,7 +135,7 @@ public class DriveSubsystem extends TestableSubsystem { // Set vision measurement confidence values - driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999)); + driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7)); this.setDefaultCommand(Commands.run(new Runnable() { diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index a8bae7d..0a2425d 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -26,13 +26,13 @@ public class HopperSubsystem extends TestableSubsystem { kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)); } - public void feedKicker() {kickerMotor.set(0.5);} + public void feedKicker() {kickerMotor.set(0.75);} public void reverseKicker() {kickerMotor.set(-0.5);} public void stopKicker() {kickerMotor.stopMotor();} - public void spindexerOn() { spindexerMotor.set(0.5);} + public void spindexerOn() { spindexerMotor.set(0.75);} public void spindexerOff() { spindexerMotor.stopMotor();} From 8f5988ea1222894ac2bf402745ba6e070c7eff28 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sun, 29 Mar 2026 21:01:34 -0400 Subject: [PATCH 06/27] Changed some logging and added back in chassis deadreckoning --- .../robot/commands/AutoShootCommandChassisTurret.java | 10 ++++++---- .../java/frc/robot/subsystems/TurretSubsystem.java | 10 ++++++++-- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 5ee7c8c..15fe417 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -102,14 +102,14 @@ public class AutoShootCommandChassisTurret extends Command{ // No april tag in camera view. Rotate towards our theoretical closest tag if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){ - Logger.recordOutput("AutoAlignState", "Look for tag"); + Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning"); Pose3d botPose = driveSubsystem.getBotPose(); - //driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this + driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this } else // We have an april tag, center it to the camera frame { - Logger.recordOutput("AutoAlignState", "Center on tag"); + Logger.recordOutput("AutoAlignState-Tracking", "Camera"); // [tx, ty, tz, pitch, yaw, roll] double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); if(targetsPose.length >= 1){ @@ -119,8 +119,10 @@ public class AutoShootCommandChassisTurret extends Command{ // If robot is centered on HUB and shooter is up to speed. Run the kicker if(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){ - Logger.recordOutput("AutoAlignState", "FIRE!"); + Logger.recordOutput("AutoAlignState-Shoot", "FIRE!"); hopperSubsystem.feedKicker(); + }else{ + Logger.recordOutput("AutoAlignState-Shoot", "Waiting"); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 1f709e1..9a2838e 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -99,12 +99,18 @@ public class TurretSubsystem extends TestableSubsystem { } public void setLauncher(double speed) { + if(speed == 0) + { + launcherMotor.stopMotor(); + return; + } launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - // launcherMotor.set(speed/1000); } public boolean isLauncherUpToSpeed() { - return true; + // Valid if velcity is within 10% of setpoint. + // May need to lower this + return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1; } public void turretControl(double position) From 9ab035db477117c10dd9ace6ef6ff64fb308d588 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Mon, 30 Mar 2026 20:26:49 -0400 Subject: [PATCH 07/27] Added back intake command and intake deploy --- src/main/java/frc/robot/RobotContainer.java | 10 ++---- .../java/frc/robot/commands/ManualIntake.java | 2 +- .../frc/robot/subsystems/IntakeSubsystem.java | 33 ++++++++++++------- 3 files changed, 25 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2e5bbb6..a38d0fa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,14 +62,8 @@ public class RobotContainer { m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); - // m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); - // m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); - - - - m_driverController.b().whileTrue(turretSubsystem.setShooter(1500)); - m_driverController.x().whileTrue(turretSubsystem.setShooter(2000)); - m_driverController.y().whileTrue(turretSubsystem.setShooter(3000)); + m_driverController.rightBumper().whileTrue(intakeSubsystem.intake()); + m_driverController.y().onTrue(intakeSubsystem.deploy()); m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } diff --git a/src/main/java/frc/robot/commands/ManualIntake.java b/src/main/java/frc/robot/commands/ManualIntake.java index 39c9aa4..95096a8 100644 --- a/src/main/java/frc/robot/commands/ManualIntake.java +++ b/src/main/java/frc/robot/commands/ManualIntake.java @@ -36,7 +36,7 @@ public class ManualIntake extends Command { public void execute() { if(m_XboxController.getYButton()==true) { - m_subsystem.extend(); + m_subsystem.deploy(); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index a200006..04122df 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -17,6 +17,7 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import static org.fairportrobotics.frc.robolib.motors.Utils.*; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; public class IntakeSubsystem extends TestableSubsystem { /** Creates a new ExampleSubsystem. */ @@ -35,9 +36,24 @@ public class IntakeSubsystem extends TestableSubsystem { deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED); } - public void extend() + public Command deploy() { - deployMotor.set(1); + return Commands.sequence( + Commands.deadline( + Commands.waitSeconds(2), + this.runEnd( + () -> {deployMotor.set(-0.5);}, + () -> {deployMotor.stopMotor();} + ) + ), + Commands.deadline( + Commands.waitSeconds(2), + this.runEnd( + () -> { deployMotor.set(0.5); }, + () -> { deployMotor.stopMotor(); } + ) + ) + ); } public void setSpeed(double speed) @@ -45,19 +61,14 @@ public class IntakeSubsystem extends TestableSubsystem { intakeMotor.set(speed); } - public Command startSpeedCommand() + public Command intake() { - return this.runOnce(() -> setSpeed(-.5)); + return this.runEnd(() -> setSpeed(-.5), () -> { intakeMotor.stopMotor();}); } - public Command killSpeedCommand() + public Command reverseIntake() { - return this.runOnce(() -> setSpeed(0)); - } - - public Command revSpeedCommand() - { - return this.runOnce(() -> setSpeed(.5)); + return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();}); } @PostTest(enabled = true) From 7d42d11db14c4ebd72e8fd80e8a9b2df95f70a9c Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Mon, 30 Mar 2026 20:29:37 -0400 Subject: [PATCH 08/27] Fixed intake commands --- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 04122df..97d1ee4 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -42,14 +42,14 @@ public class IntakeSubsystem extends TestableSubsystem { Commands.deadline( Commands.waitSeconds(2), this.runEnd( - () -> {deployMotor.set(-0.5);}, + () -> {deployMotor.set(0.5);}, // Bump this speed up if intake doesn't deploy () -> {deployMotor.stopMotor();} ) ), Commands.deadline( Commands.waitSeconds(2), this.runEnd( - () -> { deployMotor.set(0.5); }, + () -> { deployMotor.set(-0.5); }, // Same here () -> { deployMotor.stopMotor(); } ) ) From 870034bf980115ad505ea8918a1c95e7af422e53 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 31 Mar 2026 18:19:38 -0400 Subject: [PATCH 09/27] Change controler button --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a38d0fa..03a979f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,7 +64,7 @@ public class RobotContainer { m_driverController.rightBumper().whileTrue(intakeSubsystem.intake()); m_driverController.y().onTrue(intakeSubsystem.deploy()); - m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } /** From 116a6505a25cc866bdc8742d5b6d693afdf190dc Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 31 Mar 2026 18:48:56 -0400 Subject: [PATCH 10/27] change --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 03a979f..22b554e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -31,7 +32,6 @@ public class RobotContainer { new CommandXboxController(OperatorConstants.kDriverControllerPort); // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort); - public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController); public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); @@ -54,7 +54,7 @@ public class RobotContainer { * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight * joysticks}. - */ + * */ private void configureBindings() { m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); From 2de2287b9b3a102ff08bfd77a32067fe538c073b Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 31 Mar 2026 18:55:32 -0400 Subject: [PATCH 11/27] Fix --- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index b12b5a6..52f6a87 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -159,7 +159,7 @@ public class TurretSubsystem extends TestableSubsystem { }, this); } - public void startHoming() + public void homeTurret() { turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); turretState = TurretState.HOMING; From d98cf15a8dbefdc0aaa084c208a0ca2b514f74da Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 31 Mar 2026 18:59:26 -0400 Subject: [PATCH 12/27] turret lock --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 847174d..ce3483d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ package frc.robot; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; @@ -79,6 +80,6 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return driveSubsystem.getAutoCommand(); + return Commands.parallel(Commands.run(()-> {turretSubsystem.homeTurret();}, turretSubsystem), driveSubsystem.getAutoCommand()); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 52f6a87..6e0e6a1 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -206,7 +206,7 @@ public class TurretSubsystem extends TestableSubsystem { .withReverseSoftLimitEnable(true) .withReverseSoftLimitThreshold(reverseLimit) ); - // setTurretRotation(Angle.ofRelativeUnits(0, Units.Degrees)); + setTurretRotation(Angle.ofRelativeUnits(180, Units.Degrees)); } } From 4598a89b991dd42587f6816e3b55b672b1a7f38e Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 31 Mar 2026 19:13:09 -0400 Subject: [PATCH 13/27] fixed the screwed up thingie --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce3483d..ed02393 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,6 +80,6 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Commands.parallel(Commands.run(()-> {turretSubsystem.homeTurret();}, turretSubsystem), driveSubsystem.getAutoCommand()); + return Commands.parallel(Commands.run(()-> {/*turretSubsystem.homeTurret();*/}, turretSubsystem), driveSubsystem.getAutoCommand()); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6e0e6a1..6dbab83 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -206,7 +206,7 @@ public class TurretSubsystem extends TestableSubsystem { .withReverseSoftLimitEnable(true) .withReverseSoftLimitThreshold(reverseLimit) ); - setTurretRotation(Angle.ofRelativeUnits(180, Units.Degrees)); + //setTurretRotation(Angle.ofRelativeUnits(180, Units.Degrees)); } } From fa9d628c9c7526ced46dfbb86c942d648d03601a Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 31 Mar 2026 19:22:24 -0400 Subject: [PATCH 14/27] toggle --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ed02393..3f79e09 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,7 +68,7 @@ public class RobotContainer { m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); - m_driverController.rightBumper().whileTrue(intakeSubsystem.intake()); + m_driverController.rightBumper().onTrue(intakeSubsystem.intake()); m_driverController.y().onTrue(intakeSubsystem.deploy()); m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 97d1ee4..1b8ebde 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -63,7 +63,7 @@ public class IntakeSubsystem extends TestableSubsystem { public Command intake() { - return this.runEnd(() -> setSpeed(-.5), () -> { intakeMotor.stopMotor();}); + return this.runOnce(() -> {if(intakeMotor.get()!=0){setSpeed(-.5);}else{intakeMotor.stopMotor();}}); } public Command reverseIntake() From 0ced1e4d59be29bf632d4492624f13106545493a Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 2 Apr 2026 19:29:42 -0400 Subject: [PATCH 15/27] Added relay command, and many attempts to fix auto align --- lib/RoboLib | 2 +- simgui-ds.json | 22 ++- .../pathplanner/autos/Shoot 8. Hub.auto | 19 +++ .../deploy/pathplanner/paths/M. Depot. .path | 2 +- .../pathplanner/paths/Relay. R-Tr..path | 8 +- .../paths/S. Hub. shoot 8. E-Hub.path | 8 +- .../pathplanner/paths/S. R-Trench. E-Hub.path | 2 +- src/main/java/frc/robot/Constants.java | 27 ++++ src/main/java/frc/robot/RobotContainer.java | 19 ++- .../AutoShootCommandChassisTurret.java | 9 +- ...oShootCommandChassisTurretPathPlanner.java | 130 ++++++++++++++++++ .../frc/robot/commands/AutoTurretCommand.java | 12 +- .../java/frc/robot/commands/RelayCommand.java | 59 ++++++++ .../frc/robot/subsystems/DriveSubsystem.java | 12 +- .../frc/robot/subsystems/IntakeSubsystem.java | 19 ++- .../frc/robot/subsystems/TurretSubsystem.java | 36 +++-- 16 files changed, 324 insertions(+), 62 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java create mode 100644 src/main/java/frc/robot/commands/RelayCommand.java diff --git a/lib/RoboLib b/lib/RoboLib index cd9dee7..0584a07 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit cd9dee709893a27de40fb86e441223dd4c068b12 +Subproject commit 0584a07e801a1823e4113c16e42b4d97f147cc51 diff --git a/simgui-ds.json b/simgui-ds.json index e3a2a39..512b0fe 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -7,17 +12,20 @@ "incKey": 68 }, { - "decKey": 87, - "incKey": 83 + "decKey": 83, + "incKey": 87 }, { - "decKey": 69, - "decayRate": 0.0, - "incKey": 82, + "decayRate": 0.009999999776482582, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, @@ -91,7 +99,7 @@ ], "robotJoysticks": [ { - "guid": "030000004c050000cc09000011810000" + "guid": "Keyboard0" } ] } diff --git a/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto b/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto index a7380ff..216b0a1 100644 --- a/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto +++ b/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto @@ -9,6 +9,25 @@ "data": { "pathName": "S. Hub. shoot 8. E-Hub" } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + }, + { + "type": "named", + "data": { + "name": "ChassisAlignShoot" + } + } + ] + } } ] } diff --git a/src/main/deploy/pathplanner/paths/M. Depot. .path b/src/main/deploy/pathplanner/paths/M. Depot. .path index 990a3af..2e111ae 100644 --- a/src/main/deploy/pathplanner/paths/M. Depot. .path +++ b/src/main/deploy/pathplanner/paths/M. Depot. .path @@ -36,7 +36,7 @@ "y": 7.138469879518072 }, "prevControl": { - "x": 0.4124120733073493, + "x": 0.41241207330734936, "y": 6.876706525398199 }, "nextControl": { diff --git a/src/main/deploy/pathplanner/paths/Relay. R-Tr..path b/src/main/deploy/pathplanner/paths/Relay. R-Tr..path index 1edcd79..abea361 100644 --- a/src/main/deploy/pathplanner/paths/Relay. R-Tr..path +++ b/src/main/deploy/pathplanner/paths/Relay. R-Tr..path @@ -37,7 +37,7 @@ }, "prevControl": { "x": 7.691609840125085, - "y": 7.387443181134998 + "y": 7.387443181134999 }, "nextControl": { "x": 7.64634196710383, @@ -56,7 +56,7 @@ "y": 7.138469879518072 }, "nextControl": { - "x": 6.511975903614461, + "x": 6.511975903614458, "y": 7.138469879518072 }, "isLocked": false, @@ -85,11 +85,11 @@ }, "prevControl": { "x": 6.020824888067715, - "y": 0.8531028227756468 + "y": 0.8531028227756466 }, "nextControl": { "x": 5.667271497474452, - "y": 1.2066562133689314 + "y": 1.2066562133689316 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path b/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path index 8793d07..87aa838 100644 --- a/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path +++ b/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.473, - "y": 4.013 + "x": 2.9483700538099855, + "y": 4.00221686746988 }, "prevControl": { - "x": 3.1233132530120478, - "y": 4.034855421686746 + "x": 2.5986833068220334, + "y": 4.024072289156626 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path b/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path index 8fdebe3..8b6f670 100644 --- a/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path +++ b/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path @@ -24,7 +24,7 @@ "y": 3.77441480327739 }, "nextControl": { - "x": 2.348744281004639, + "x": 2.3487442810046395, "y": 4.0114647147949 }, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 39b2ea8..d0cccb7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,12 @@ package frc.robot; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; @@ -76,6 +82,19 @@ public final class Constants { public static final int BACK_RIGHT_DRIVE_ID = 42; public static final int BACK_RIGHT_ENCODER_ID = 43; } + + public static class RobotChassisLimits{ + // Max velocity in meters per second + public static final double MAX_ROBOT_LINEAR_VELOCITY = 3; + // In meters per second squared + public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2; + + // In radians per second + public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI; + // In radians per second squared + public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI; + } + //Bird public static class ExtraIDEntities { public static final int PIGEON_ID = 1; @@ -121,4 +140,12 @@ public final class Constants { public static final int SPINDEXER_MOTOR_ID = 8; public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; } + + public static class FieldPoses { + private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3f79e09..9d8bb69 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc.robot; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -11,10 +13,12 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.AutoShootCommandChassisTurret; +import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner; import frc.robot.commands.AutoTurretCommand; import frc.robot.commands.ManualHopperCommand; import frc.robot.commands.ManualIntake; import frc.robot.commands.ManualShootCommand; +import frc.robot.commands.RelayCommand; import frc.robot.commands.SetDeflectorCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -45,11 +49,14 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); + // turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); // turretSubsystem.homeTurret(); // Configure the trigger bindings configureBindings(); + + NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + } /** @@ -68,8 +75,14 @@ public class RobotContainer { m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); + m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); + // m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); + m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + + m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); m_driverController.rightBumper().onTrue(intakeSubsystem.intake()); - m_driverController.y().onTrue(intakeSubsystem.deploy()); + m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem)); + m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } @@ -80,6 +93,6 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Commands.parallel(Commands.run(()-> {/*turretSubsystem.homeTurret();*/}, turretSubsystem), driveSubsystem.getAutoCommand()); + return Commands.parallel(intakeSubsystem.deploy(), Commands.run(()-> {turretSubsystem.homeTurret();}, turretSubsystem), driveSubsystem.getAutoCommand()); } } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index d7ca341..a524d0e 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -75,6 +75,7 @@ public class AutoShootCommandChassisTurret extends Command{ }); turretSubsystem.setLauncher(5500); + turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); hopperSubsystem.spindexerOn(); cameraAutoCenterController.calculate(-100); @@ -91,8 +92,12 @@ public class AutoShootCommandChassisTurret extends Command{ Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); - // Add 180 because we want the back of the robot to face the HUB - deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); + DriverStation.getAlliance().ifPresent((aliance) -> { + if (aliance == Alliance.Blue) { + // Add 180 because we want the back of the robot to face the HUB + deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); + } + }); } @Override diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java new file mode 100644 index 0000000..93238d5 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java @@ -0,0 +1,130 @@ +package frc.robot.commands; + +import java.util.Arrays; +import java.util.List; + +import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; +import org.littletonrobotics.junction.Logger; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Constants; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.TurretSubsystem; + +public class AutoShootCommandChassisTurretPathPlanner extends Command { + + // For Chassis Control + private DriveSubsystem driveSubsystem; + + private HopperSubsystem hopperSubsystem; + private TurretSubsystem turretSubsystem; + + private int[] tagFilters; + + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + private Pose3d closestTag; + private Pose2d targetPPPose; + + private Command pathFollowCommand; + + public AutoShootCommandChassisTurretPathPlanner(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, + TurretSubsystem turretSubsystem) { + this.driveSubsystem = driveSubsystem; + this.hopperSubsystem = hopperSubsystem; + this.turretSubsystem = turretSubsystem; + + this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem); + + } + + @Override + public void initialize() { + + DriverStation.getAlliance().ifPresent((aliance) -> { + Pose3d targetHub; + if (aliance == Alliance.Blue) { + tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; + targetHub = Constants.FieldPoses.BLUE_HUB_POSE; + } else { + tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; + targetHub = Constants.FieldPoses.RED_HUB_POSE; + } + + Pose3d botPose = driveSubsystem.getBotPose(); + // Find closest target to us + List tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()) + .toList(); + closestTag = botPose.nearest(tagPoses); + Logger.recordOutput("AutoAlignPP-ClosestAprilTagPose", closestTag); + + Translation2d delta = botPose.getTranslation().toTranslation2d() + .minus(closestTag.getTranslation().toTranslation2d()); + Logger.recordOutput("AutoAlignPP-ClosestAprilTagTransform", delta); + + // Add 180 because we want the back of the robot to face the HUB + // deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); + + // Translate back 1 meter from HUB + Translation2d newTrans = targetHub.getTranslation().toTranslation2d() + .plus(new Translation2d(1.5, delta.getAngle())); + targetPPPose = new Pose2d(newTrans, delta.getAngle()); + Logger.recordOutput("AutoAlignPP-PPTargetPose", targetPPPose); + + pathFollowCommand = AutoBuilder.pathfindToPose(targetPPPose, + new PathConstraints(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY, + Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL, + Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY, + Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL)); + + CommandScheduler.getInstance().schedule(pathFollowCommand); + }); + + turretSubsystem.setLauncher(5500); + hopperSubsystem.spindexerOn(); + + } + + @Override + public void execute() { + + // If robot is centered on HUB and shooter is up to speed. Run the kicker + if (turretSubsystem.isLauncherUpToSpeed() && pathFollowCommand.isFinished()) { + Logger.recordOutput("AutoAlignState-Shoot", "FIRE!"); + hopperSubsystem.feedKicker(); + } else { + Logger.recordOutput("AutoAlignState-Shoot", "Waiting"); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + pathFollowCommand.cancel(); + turretSubsystem.setLauncher(0); + + hopperSubsystem.stopKicker(); + hopperSubsystem.spindexerOff(); + + driveSubsystem.stopDrive(); + + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, + Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER); + } + +} diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index da975ef..16455a0 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -29,8 +29,6 @@ public class AutoTurretCommand extends Command { private int[] shootingTagFilters; - private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); - private Pose3d closestTag; private Pose3d scoringHubPose; @@ -49,14 +47,10 @@ public class AutoTurretCommand extends Command { DriverStation.getAlliance().ifPresent((aliance) -> { if (aliance == Alliance.Blue) { shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; - - scoringHubPose = fieldTags.getTagPose(26).get(); - scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE; } else { shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; - - scoringHubPose = fieldTags.getTagPose(10).get(); - scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + scoringHubPose = Constants.FieldPoses.RED_HUB_POSE; } Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose); }); @@ -76,7 +70,7 @@ public class AutoTurretCommand extends Command { if(turretSubsystem.isTurretReady()){ - turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure()); + turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure()); // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees)); } diff --git a/src/main/java/frc/robot/commands/RelayCommand.java b/src/main/java/frc/robot/commands/RelayCommand.java new file mode 100644 index 0000000..754f9d7 --- /dev/null +++ b/src/main/java/frc/robot/commands/RelayCommand.java @@ -0,0 +1,59 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.TurretSubsystem; + +public class RelayCommand extends Command{ + + private HopperSubsystem hopperSubsystem; + private TurretSubsystem turretSubsystem; + private IntakeSubsystem intakeSubsystem; + + private Command waitCommand = Commands.waitSeconds(1.5); + + public RelayCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem){ + this.hopperSubsystem = hopperSubsystem; + this.turretSubsystem = turretSubsystem; + this.intakeSubsystem = intakeSubsystem; + + addRequirements(hopperSubsystem, turretSubsystem, intakeSubsystem); + } + + @Override + public void initialize() { + turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3); + turretSubsystem.setLauncher(5500); + hopperSubsystem.spindexerOn(); + CommandScheduler.getInstance().schedule(waitCommand); + } + + @Override + public void execute() { + if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){ + hopperSubsystem.feedKicker(); + }else{ + hopperSubsystem.stopKicker(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + waitCommand.cancel(); + hopperSubsystem.spindexerOff(); + hopperSubsystem.stopKicker(); + + turretSubsystem.setLauncher(0); + turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); + } + +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 6dbad45..d5066ca 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -10,6 +10,8 @@ import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; import org.littletonrobotics.junction.Logger; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -50,8 +52,8 @@ public class DriveSubsystem extends TestableSubsystem { driveSystem = swerveBuilder .withCanbusName("Drive") .withPigeonId(Constants.ExtraIDEntities.PIGEON_ID) - .withMaxLinearVelocity(3.0) - .withMaxAngularVelocity(Math.PI) + .withMaxLinearVelocity(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY) + .withMaxAngularVelocity(Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY) .withSwerveModule( swerveBuilder.new SwerveModuleBuilder() .withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID) @@ -135,7 +137,7 @@ public class DriveSubsystem extends TestableSubsystem { // Set vision measurement confidence values - driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7)); + driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999)); this.setDefaultCommand(Commands.run(new Runnable() { @@ -163,8 +165,8 @@ public class DriveSubsystem extends TestableSubsystem { driveSystem::getRobotRelativeSpeeds, (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()), new PPHolonomicDriveController( - new PIDConstants(5.0, 0, 0), - new PIDConstants(5.0, 0, 0)), + new PIDConstants(1.5, 0, 0), + new PIDConstants(Math.PI/2, 0, 0)), config, () -> { var alliance = DriverStation.getAlliance(); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1b8ebde..7d0e930 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -41,21 +41,18 @@ public class IntakeSubsystem extends TestableSubsystem { return Commands.sequence( Commands.deadline( Commands.waitSeconds(2), - this.runEnd( - () -> {deployMotor.set(0.5);}, // Bump this speed up if intake doesn't deploy - () -> {deployMotor.stopMotor();} + this.runOnce( + () -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy ) ), - Commands.deadline( - Commands.waitSeconds(2), - this.runEnd( - () -> { deployMotor.set(-0.5); }, // Same here - () -> { deployMotor.stopMotor(); } - ) - ) + this.runOnce(() -> deployMotor.stopMotor()) ); } + public Command resetDeploy(){ + return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor()); + } + public void setSpeed(double speed) { intakeMotor.set(speed); @@ -63,7 +60,7 @@ public class IntakeSubsystem extends TestableSubsystem { public Command intake() { - return this.runOnce(() -> {if(intakeMotor.get()!=0){setSpeed(-.5);}else{intakeMotor.stopMotor();}}); + return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}}); } public Command reverseIntake() diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6dbab83..99a289f 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -74,10 +74,10 @@ public class TurretSubsystem extends TestableSubsystem { new TalonFXConfiguration() .withSlot0( new Slot0Configs() - .withKP(2) + .withKP(3) .withKI(1) .withKD(0) - .withKS(0) + .withKS(0.3) ) .withMotorOutput( new MotorOutputConfigs() @@ -134,11 +134,6 @@ public class TurretSubsystem extends TestableSubsystem { return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1; } - public void turretControl(double position) - { - turretMotor.setControl(new PositionVoltage(position)); - } - public void setTargetElevation(double elev) { double pos = Math.abs((elev - 300)/300); @@ -161,6 +156,9 @@ public class TurretSubsystem extends TestableSubsystem { public void homeTurret() { + if(turretState != TurretState.INIT){ + return; // Early return, we don't want to home if we already have + } turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); turretState = TurretState.HOMING; } @@ -193,7 +191,7 @@ public class TurretSubsystem extends TestableSubsystem { if(turretLimitSwitch.get()) { // Turret has tripped the switch turretMotor.stopMotor(); - turretOffset = turretMotorPosition.refresh().getValue().plus(Angle.ofRelativeUnits(100, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); + turretOffset = turretMotorPosition.refresh().getValue().minus(Angle.ofRelativeUnits(65, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); turretState = TurretState.READY; Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset); Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset); @@ -201,12 +199,13 @@ public class TurretSubsystem extends TestableSubsystem { Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit); turretMotor.getConfigurator().apply( new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) + // .withForwardSoftLimitEnable(true) .withForwardSoftLimitThreshold(forwardLimit) - .withReverseSoftLimitEnable(true) + // .withReverseSoftLimitEnable(true) .withReverseSoftLimitThreshold(reverseLimit) ); - //setTurretRotation(Angle.ofRelativeUnits(180, Units.Degrees)); + // setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees)); + setTurretMotorRotation(Angle.ofRelativeUnits(0, Units.Degrees)); } } @@ -226,7 +225,16 @@ public class TurretSubsystem extends TestableSubsystem { } - public void setTurretRotation(Angle pos) + public void setTurretFieldRelative(Angle pos) + { + if(turretState != TurretState.READY) + { + // Not ready to set angle + return; + } + } + + public void setTurretRobotRelative(Angle pos) { if(turretState != TurretState.READY) { @@ -237,8 +245,8 @@ public class TurretSubsystem extends TestableSubsystem { setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); } - public void setTurretMotorRotation(Angle pos){ - turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset))); + private void setTurretMotorRotation(Angle pos){ + turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1))); turretSim.setRawRotorPosition(pos.plus(turretOffset)); } From 2f981f2d09fccad6f54808bfdeb367cdf8215db0 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 2 Apr 2026 19:42:26 -0400 Subject: [PATCH 16/27] Hopefully working auto align --- .../robot/commands/AutoShootCommandChassisTurret.java | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index a524d0e..0ceeee2 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -59,7 +59,7 @@ public class AutoShootCommandChassisTurret extends Command{ cameraAutoCenterController.setSetpoint(0); - deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0); + deadreckoningAutoCenterController = new PIDController(Math.PI, 0, 0.0); deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI); } @@ -92,12 +92,7 @@ public class AutoShootCommandChassisTurret extends Command{ Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); - DriverStation.getAlliance().ifPresent((aliance) -> { - if (aliance == Alliance.Blue) { - // Add 180 because we want the back of the robot to face the HUB - deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); - } - }); + deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians()); } @Override @@ -111,7 +106,7 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning"); Pose3d botPose = driveSubsystem.getBotPose(); - driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); + driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); } else // We have an april tag, center it to the camera frame { From a5cf72bc3862f415bfdeb27f5dd997511c974f6f Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 3 Apr 2026 15:55:07 -0400 Subject: [PATCH 17/27] Eh --- lib/RoboLib | 2 +- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../AutoShootCommandChassisTurret.java | 15 ++++++++----- ...oShootCommandChassisTurretPathPlanner.java | 2 +- .../frc/robot/commands/AutoTurretCommand.java | 2 +- .../java/frc/robot/commands/RelayCommand.java | 4 ++++ .../frc/robot/subsystems/HopperSubsystem.java | 3 +++ .../frc/robot/subsystems/TurretSubsystem.java | 21 ++++++++++++------- 9 files changed, 36 insertions(+), 19 deletions(-) diff --git a/lib/RoboLib b/lib/RoboLib index 0584a07..840c851 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit 0584a07e801a1823e4113c16e42b4d97f147cc51 +Subproject commit 840c851253187081a50ca9492c032cec8d376e7b diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d0cccb7..e0b4832 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -129,7 +129,7 @@ public final class Constants { public static final double DEFLECTOR_STORED_ANGLE = 0; public static final double DEFLECTOR_SET_ANGLE1 = 100; public static final double DEFLECTOR_SET_ANGLE2 = 200; - public static final double DEFLECTOR_SET_ANGLE3 = 300; + public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed public static final double DEFLECTOR_SERVO_RATIO = 1.0; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d8bb69..88b10f0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -76,8 +76,8 @@ public class RobotContainer { m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); - // m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); - m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + //m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); + // m_driverController:wa.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); m_driverController.rightBumper().onTrue(intakeSubsystem.intake()); diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 0ceeee2..9b41e29 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -54,12 +54,12 @@ public class AutoShootCommandChassisTurret extends Command{ this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem); - cameraAutoCenterController = new PIDController(0.1, 0, 0.0); - cameraAutoCenterController.setTolerance(0.2); // Units is meters + cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0); + cameraAutoCenterController.setTolerance(0.2); // Units degrees cameraAutoCenterController.setSetpoint(0); - deadreckoningAutoCenterController = new PIDController(Math.PI, 0, 0.0); + deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0); deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI); } @@ -80,7 +80,7 @@ public class AutoShootCommandChassisTurret extends Command{ cameraAutoCenterController.calculate(-100); // Change to HUB tracking pipeline - LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER); + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER); LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters); Pose3d botPose = driveSubsystem.getBotPose(); @@ -106,7 +106,7 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning"); Pose3d botPose = driveSubsystem.getBotPose(); - driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); + //driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); } else // We have an april tag, center it to the camera frame { @@ -114,6 +114,11 @@ public class AutoShootCommandChassisTurret extends Command{ // [tx, ty, tz, pitch, yaw, roll] double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); if(targetsPose.length >= 1){ + Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]); + + // Calculate turret Angle + double targetDistance = targetsPose[2]; + driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated } } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java index 93238d5..683a056 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java @@ -115,7 +115,7 @@ public class AutoShootCommandChassisTurretPathPlanner extends Command { @Override public void end(boolean interrupted) { - pathFollowCommand.cancel(); + CommandScheduler.getInstance().cancel(pathFollowCommand); turretSubsystem.setLauncher(0); hopperSubsystem.stopKicker(); diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index 16455a0..6ce9eee 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -62,7 +62,7 @@ public class AutoTurretCommand extends Command { Pose3d botPose = driveSubsystem.getBotPose(); Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), - new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngle().in(Units.Rotations))))); + new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative().in(Units.Rotations))))); Logger.recordOutput("AutoTurret-TurretPose", turretPose); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); diff --git a/src/main/java/frc/robot/commands/RelayCommand.java b/src/main/java/frc/robot/commands/RelayCommand.java index 754f9d7..80f80ce 100644 --- a/src/main/java/frc/robot/commands/RelayCommand.java +++ b/src/main/java/frc/robot/commands/RelayCommand.java @@ -1,5 +1,7 @@ package frc.robot.commands; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -30,6 +32,7 @@ public class RelayCommand extends Command{ turretSubsystem.setLauncher(5500); hopperSubsystem.spindexerOn(); CommandScheduler.getInstance().schedule(waitCommand); + turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(-6, Units.Degrees)); // Return to 0 after MIAMI VALLEY } @Override @@ -54,6 +57,7 @@ public class RelayCommand extends Command{ turretSubsystem.setLauncher(0); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); + turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY } } diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 0a2425d..6f0f1d5 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -7,6 +7,7 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.hardware.TalonFX; @@ -21,6 +22,8 @@ public class HopperSubsystem extends TestableSubsystem { public HopperSubsystem() { spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)); + // spindexerMotor.getConfigurator().apply(new ClosedLoopGeneralConfigs() + // .); kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 99a289f..fe227ea 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -75,7 +75,7 @@ public class TurretSubsystem extends TestableSubsystem { .withSlot0( new Slot0Configs() .withKP(3) - .withKI(1) + .withKI(1.5) .withKD(0) .withKS(0.3) ) @@ -172,7 +172,7 @@ public class TurretSubsystem extends TestableSubsystem { turretMotor.set(speed); } - public Angle getTurretAngle() + public Angle getTurretAngleRobotRelative() { return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO); } @@ -200,12 +200,12 @@ public class TurretSubsystem extends TestableSubsystem { turretMotor.getConfigurator().apply( new SoftwareLimitSwitchConfigs() // .withForwardSoftLimitEnable(true) - .withForwardSoftLimitThreshold(forwardLimit) + // .withForwardSoftLimitThreshold(forwardLimit) // .withReverseSoftLimitEnable(true) - .withReverseSoftLimitThreshold(reverseLimit) + // .withReverseSoftLimitThreshold(reverseLimit) ); // setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees)); - setTurretMotorRotation(Angle.ofRelativeUnits(0, Units.Degrees)); + setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY } } @@ -214,10 +214,11 @@ public class TurretSubsystem extends TestableSubsystem { Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get()); Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); Logger.recordOutput("TurretSubsystem-TurretState", turretState); - Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees)); - Logger.recordOutput("TurretSubsystem-TurretRequestedPos", turretControlMode.getPositionMeasure().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffset.in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue()); @@ -245,11 +246,15 @@ public class TurretSubsystem extends TestableSubsystem { setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); } - private void setTurretMotorRotation(Angle pos){ + public void setTurretMotorRotation(Angle pos){ turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1))); turretSim.setRawRotorPosition(pos.plus(turretOffset)); } + private Angle getRequestedPosTurretRelative() { + return turretControlMode.getPositionMeasure().minus(turretOffset).div(Constants.ShooterConstants.TURRET_GEAR_RATIO); + } + @PostTest() public void TurretSubsystem_CANDevicesConnected(){ assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse(); From 624420ad68fd845521984a3a9e0d51933fc3946b Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 3 Apr 2026 16:51:47 -0400 Subject: [PATCH 18/27] Turret in Rotations units now --- .../AutoShootCommandChassisTurret.java | 2 +- .../frc/robot/commands/AutoTurretCommand.java | 4 +- .../java/frc/robot/commands/RelayCommand.java | 6 +-- .../frc/robot/subsystems/TurretSubsystem.java | 48 +++++++++---------- 4 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 9b41e29..3aa0432 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -74,7 +74,7 @@ public class AutoShootCommandChassisTurret extends Command{ } }); - turretSubsystem.setLauncher(5500); + turretSubsystem.setLauncher(6000); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); hopperSubsystem.spindexerOn(); cameraAutoCenterController.calculate(-100); diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index 6ce9eee..f4498b0 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -62,7 +62,7 @@ public class AutoTurretCommand extends Command { Pose3d botPose = driveSubsystem.getBotPose(); Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), - new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative().in(Units.Rotations))))); + new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative())))); Logger.recordOutput("AutoTurret-TurretPose", turretPose); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); @@ -70,7 +70,7 @@ public class AutoTurretCommand extends Command { if(turretSubsystem.isTurretReady()){ - turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure()); + // turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure()); // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees)); } diff --git a/src/main/java/frc/robot/commands/RelayCommand.java b/src/main/java/frc/robot/commands/RelayCommand.java index 80f80ce..33648e4 100644 --- a/src/main/java/frc/robot/commands/RelayCommand.java +++ b/src/main/java/frc/robot/commands/RelayCommand.java @@ -29,10 +29,10 @@ public class RelayCommand extends Command{ @Override public void initialize() { turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3); - turretSubsystem.setLauncher(5500); + turretSubsystem.setLauncher(6000); hopperSubsystem.spindexerOn(); CommandScheduler.getInstance().schedule(waitCommand); - turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(-6, Units.Degrees)); // Return to 0 after MIAMI VALLEY + turretSubsystem.setTurretMotorRotation(1); // Return to 0 after MIAMI VALLEY } @Override @@ -57,7 +57,7 @@ public class RelayCommand extends Command{ turretSubsystem.setLauncher(0); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); - turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY + turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index fe227ea..95c3437 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -61,7 +61,7 @@ public class TurretSubsystem extends TestableSubsystem { private SparkMax launcherMotor; private SparkClosedLoopController launcherControler; - private Angle turretOffset = Angle.ofBaseUnits(0, Units.Degrees); + private double turretOffsetInRotations = 0; private Servo hood; @@ -131,7 +131,7 @@ public class TurretSubsystem extends TestableSubsystem { public boolean isLauncherUpToSpeed() { // Valid if velcity is within 10% of setpoint. // May need to lower this - return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1; + return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.075; } public void setTargetElevation(double elev) @@ -172,13 +172,13 @@ public class TurretSubsystem extends TestableSubsystem { turretMotor.set(speed); } - public Angle getTurretAngleRobotRelative() + public double getTurretAngleRobotRelative() { - return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO); + return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO; } - private Angle getTurretMotorPosition(){ - return turretMotorPosition.refresh().getValue().minus(turretOffset); + private double getTurretMotorPosition(){ + return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations; } @Override @@ -191,12 +191,12 @@ public class TurretSubsystem extends TestableSubsystem { if(turretLimitSwitch.get()) { // Turret has tripped the switch turretMotor.stopMotor(); - turretOffset = turretMotorPosition.refresh().getValue().minus(Angle.ofRelativeUnits(65, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); + turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323); turretState = TurretState.READY; - Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset); - Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset); - Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit); - Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit); + // Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset); + // Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset); + // Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit); + // Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit); turretMotor.getConfigurator().apply( new SoftwareLimitSwitchConfigs() // .withForwardSoftLimitEnable(true) @@ -205,7 +205,7 @@ public class TurretSubsystem extends TestableSubsystem { // .withReverseSoftLimitThreshold(reverseLimit) ); // setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees)); - setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY + setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY } } @@ -214,19 +214,19 @@ public class TurretSubsystem extends TestableSubsystem { Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get()); Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); Logger.recordOutput("TurretSubsystem-TurretState", turretState); - Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative().in(Units.Degrees)); - Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative()); + Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition()); Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees)); - Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative().in(Units.Degrees)); - Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffset.in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative()); + Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations); Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue()); } } - public void setTurretFieldRelative(Angle pos) + public void setTurretFieldRelative(double pos) { if(turretState != TurretState.READY) { @@ -235,7 +235,7 @@ public class TurretSubsystem extends TestableSubsystem { } } - public void setTurretRobotRelative(Angle pos) + public void setTurretRobotRelative(double pos) { if(turretState != TurretState.READY) { @@ -243,16 +243,16 @@ public class TurretSubsystem extends TestableSubsystem { return; } - setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); + setTurretMotorRotation(pos * Constants.ShooterConstants.TURRET_GEAR_RATIO); } - public void setTurretMotorRotation(Angle pos){ - turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1))); - turretSim.setRawRotorPosition(pos.plus(turretOffset)); + public void setTurretMotorRotation(double pos){ + turretMotor.setControl(turretControlMode.withPosition((pos + turretOffsetInRotations))); + turretSim.setRawRotorPosition(pos + turretOffsetInRotations); } - private Angle getRequestedPosTurretRelative() { - return turretControlMode.getPositionMeasure().minus(turretOffset).div(Constants.ShooterConstants.TURRET_GEAR_RATIO); + private double getRequestedPosTurretRelative() { + return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO; } @PostTest() From 05807a663ed9b36a95f60537c24a56be312145ce Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 3 Apr 2026 17:48:56 -0400 Subject: [PATCH 19/27] Turret Auto aligns with chassis --- .../commands/AutoShootCommandChassisTurret.java | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 3aa0432..629bfe5 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -117,7 +117,18 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]); // Calculate turret Angle - double targetDistance = targetsPose[2]; + double cameraDistanceToTarget = targetsPose[2]; + double cameraToShootDistance = 0.26035; + // a^2 + b^2 = c^2 + double shooterDistanceToTarget = Math.sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2)); + double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget); + Logger.recordOutput("AutoAlign-TurretAngleOfTriagle", angleAtShooter); + double robotTurretAngle = (Math.PI/2) - angleAtShooter; + Logger.recordOutput("AutoAlign-TurretAngleRelativeToRobot", robotTurretAngle); + double robotTurretRotations = (robotTurretAngle / Math.PI) * Constants.ShooterConstants.TURRET_GEAR_RATIO; + Logger.recordOutput("AutoAlign-TurretRotations", robotTurretRotations); + + turretSubsystem.setTurretMotorRotation(-robotTurretRotations); driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated } From 317d21d0003af669c10ee2071364a2a52a85af23 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sat, 4 Apr 2026 13:38:28 -0400 Subject: [PATCH 20/27] Much better Turret PID --- src/main/java/frc/robot/Utils.java | 25 +++++++++++++++++++ .../AutoShootCommandChassisTurret.java | 15 +++-------- .../frc/robot/subsystems/TurretSubsystem.java | 10 ++++---- 3 files changed, 33 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc/robot/Utils.java diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java new file mode 100644 index 0000000..64bd85f --- /dev/null +++ b/src/main/java/frc/robot/Utils.java @@ -0,0 +1,25 @@ +package frc.robot; + +public class Utils { + + /** + * Calculates the needed angle of the turret for a distance from the HUB. + * @param distanceToTag + * @return number of rotations of the turret, robot relative + */ + public static double calculateTurretAngleFromCameraTagDistance(double distanceToTag){ + + // Calculate turret Angle + double cameraDistanceToTarget = distanceToTag + .6; + double cameraToShootDistance = 0.26035; + // a^2 + b^2 = c^2 + double shooterDistanceToTarget = Math + .sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2)); + double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget); + double robotTurretAngle = (Math.PI / 2) - angleAtShooter; + double robotTurretRotations = (robotTurretAngle / Math.PI); + + return robotTurretRotations; + } + +} diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 629bfe5..0b6d64d 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; +import frc.robot.Utils; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.TurretSubsystem; @@ -117,18 +118,8 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]); // Calculate turret Angle - double cameraDistanceToTarget = targetsPose[2]; - double cameraToShootDistance = 0.26035; - // a^2 + b^2 = c^2 - double shooterDistanceToTarget = Math.sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2)); - double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget); - Logger.recordOutput("AutoAlign-TurretAngleOfTriagle", angleAtShooter); - double robotTurretAngle = (Math.PI/2) - angleAtShooter; - Logger.recordOutput("AutoAlign-TurretAngleRelativeToRobot", robotTurretAngle); - double robotTurretRotations = (robotTurretAngle / Math.PI) * Constants.ShooterConstants.TURRET_GEAR_RATIO; - Logger.recordOutput("AutoAlign-TurretRotations", robotTurretRotations); - - turretSubsystem.setTurretMotorRotation(-robotTurretRotations); + double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO; + turretSubsystem.setTurretMotorRotation(-turretAngle); driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 95c3437..3391db5 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -74,10 +74,10 @@ public class TurretSubsystem extends TestableSubsystem { new TalonFXConfiguration() .withSlot0( new Slot0Configs() - .withKP(3) - .withKI(1.5) - .withKD(0) - .withKS(0.3) + .withKP(6) + .withKI(0) + .withKD(0.3) + .withKS(0) ) .withMotorOutput( new MotorOutputConfigs() @@ -131,7 +131,7 @@ public class TurretSubsystem extends TestableSubsystem { public boolean isLauncherUpToSpeed() { // Valid if velcity is within 10% of setpoint. // May need to lower this - return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.075; + return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03; } public void setTargetElevation(double elev) From 57fa16b4b331bf2bc2c6c5c3dcca5646fb46a07b Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sat, 4 Apr 2026 15:25:52 -0400 Subject: [PATCH 21/27] Kicker PID --- .../frc/robot/subsystems/HopperSubsystem.java | 57 +++++++++++++++++-- 1 file changed, 51 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 6f0f1d5..e8e4e53 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -6,32 +6,72 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; +import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.AngularVelocityUnit; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; + import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import frc.robot.Constants; public class HopperSubsystem extends TestableSubsystem { private TalonFX spindexerMotor; + + private TalonFX kickerMotor; + private VelocityVoltage kickerControl = new VelocityVoltage(0); + private StatusSignal kickerVelocity; + /** Creates a new HopperSubsystem. */ public HopperSubsystem() { spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); - spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)); - // spindexerMotor.getConfigurator().apply(new ClosedLoopGeneralConfigs() - // .); + spindexerMotor.getConfigurator().apply( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)) + .withSlot0( + new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withKS(0))); kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); - kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)); + kickerMotor.getConfigurator().apply( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION) + ) + .withSlot0(new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withKS(0) + )); + kickerVelocity = kickerMotor.getVelocity(); } - public void feedKicker() {kickerMotor.set(0.75);} + public void feedKicker() { + kickerMotor.setControl(kickerControl.withVelocity(1000)); + // kickerMotor.set(0.75); + } - public void reverseKicker() {kickerMotor.set(-0.5);} + public void reverseKicker() { + kickerMotor.setControl(kickerControl.withVelocity(-500)); + // kickerMotor.set(-0.5); + } public void stopKicker() {kickerMotor.stopMotor();} @@ -39,6 +79,11 @@ public class HopperSubsystem extends TestableSubsystem { public void spindexerOff() { spindexerMotor.stopMotor();} + @Override + public void periodic() { + Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond)); + } + @PostTest(enabled = true) public void hopperSystem_CANDevicesConnected(){ assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue(); From eb16cf40fe636e737b723bd7d6828efa60025928 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 7 Apr 2026 19:16:20 -0400 Subject: [PATCH 22/27] Deleted ManualHopperCommand because it was never called and never meant to be permanent --- lib/RoboLib | 2 +- src/main/java/frc/robot/RobotContainer.java | 125 +++++++++--------- .../robot/commands/ManualHopperCommand.java | 58 -------- 3 files changed, 64 insertions(+), 121 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ManualHopperCommand.java diff --git a/lib/RoboLib b/lib/RoboLib index 840c851..cd9dee7 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit 840c851253187081a50ca9492c032cec8d376e7b +Subproject commit cd9dee709893a27de40fb86e441223dd4c068b12 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 88b10f0..a137a95 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,12 +1,10 @@ // 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; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -14,10 +12,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.AutoShootCommandChassisTurret; import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner; -import frc.robot.commands.AutoTurretCommand; -import frc.robot.commands.ManualHopperCommand; -import frc.robot.commands.ManualIntake; -import frc.robot.commands.ManualShootCommand; import frc.robot.commands.RelayCommand; import frc.robot.commands.SetDeflectorCommand; import frc.robot.subsystems.DriveSubsystem; @@ -26,73 +20,80 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.TurretSubsystem; /** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and trigger mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = - new CommandXboxController(OperatorConstants.kDriverControllerPort); + // The robot's subsystems and commands are defined here... + // Replace with CommandPS4Controller or CommandJoystick if needed - // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort); - public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController); + private final CommandXboxController m_driverController + = new CommandXboxController(OperatorConstants.kDriverControllerPort); - public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort); + public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController); - public final TurretSubsystem turretSubsystem = new TurretSubsystem(); + public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); - public final HopperSubsystem hopperSubsystem = new HopperSubsystem(); - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { + public final TurretSubsystem turretSubsystem = new TurretSubsystem(); - // turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); - // turretSubsystem.homeTurret(); + public final HopperSubsystem hopperSubsystem = new HopperSubsystem(); - // Configure the trigger bindings - configureBindings(); + /** + * The container for the robot. Contains subsystems, OI devices, and + * commands. + */ + public RobotContainer() { - NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + // turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); + // turretSubsystem.homeTurret(); + // Configure the trigger bindings + configureBindings(); - } + NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); - /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. - * */ + } - private void configureBindings() { - m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); - m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); - m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); - m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); - - m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); - //m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); - // m_driverController:wa.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); - - m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); - m_driverController.rightBumper().onTrue(intakeSubsystem.intake()); - m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem)); - m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); - m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. + /** + * Use this method to define your trigger->command mappings. Triggers can be + * created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor + * with an arbitrary predicate, or via the named factories in {@link + * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link + * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller + * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight + * joysticks}. * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return Commands.parallel(intakeSubsystem.deploy(), Commands.run(()-> {turretSubsystem.homeTurret();}, turretSubsystem), driveSubsystem.getAutoCommand()); - } + */ + private void configureBindings() { + m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); + m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); + m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); + m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); + + m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); + //m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); + // m_driverController:wa.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + + m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); + m_driverController.rightBumper().onTrue(intakeSubsystem.intake()); + m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem)); + m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); + m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // An example command will be run in autonomous + return Commands.parallel(intakeSubsystem.deploy(), Commands.run(() -> { + turretSubsystem.homeTurret(); + }, turretSubsystem), driveSubsystem.getAutoCommand()); + } } diff --git a/src/main/java/frc/robot/commands/ManualHopperCommand.java b/src/main/java/frc/robot/commands/ManualHopperCommand.java deleted file mode 100644 index 1c552f7..0000000 --- a/src/main/java/frc/robot/commands/ManualHopperCommand.java +++ /dev/null @@ -1,58 +0,0 @@ -// 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.commands; - -import frc.robot.subsystems.HopperSubsystem; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; - -/** An example command that uses an example subsystem. */ -public class ManualHopperCommand extends Command { - private final HopperSubsystem m_subsystem; - private final XboxController m_xboxController; - - public ManualHopperCommand(HopperSubsystem subsystem, XboxController xboxController) { - m_subsystem = subsystem; - m_xboxController = xboxController; - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_subsystem.spindexerOn(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2) - { - m_subsystem.feedKicker(); - } - else - { - m_subsystem.stopKicker(); - } - if(m_xboxController.getRightBumper()) - { - m_subsystem.spindexerOn(); - } - if(m_xboxController.getLeftBumper()) - { - m_subsystem.spindexerOff(); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} From 8cd0b5a65f5e80e36f2c1585d2f9ec6ffbf34469 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 7 Apr 2026 19:20:15 -0400 Subject: [PATCH 23/27] Removed Intake and Shoot --- .../java/frc/robot/commands/ManualIntake.java | 52 ------------------- .../robot/commands/ManualShootCommand.java | 48 ----------------- 2 files changed, 100 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ManualIntake.java delete mode 100644 src/main/java/frc/robot/commands/ManualShootCommand.java diff --git a/src/main/java/frc/robot/commands/ManualIntake.java b/src/main/java/frc/robot/commands/ManualIntake.java deleted file mode 100644 index 95096a8..0000000 --- a/src/main/java/frc/robot/commands/ManualIntake.java +++ /dev/null @@ -1,52 +0,0 @@ -// 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.commands; - -import frc.robot.subsystems.IntakeSubsystem; - -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; - -/** An example command that uses an example subsystem. */ -public class ManualIntake extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final IntakeSubsystem m_subsystem; - private final XboxController m_XboxController; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ManualIntake(IntakeSubsystem subsystem, XboxController xboxController) { - m_subsystem = subsystem; - m_XboxController = xboxController; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if(m_XboxController.getYButton()==true) - { - m_subsystem.deploy(); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ManualShootCommand.java b/src/main/java/frc/robot/commands/ManualShootCommand.java deleted file mode 100644 index cb397b0..0000000 --- a/src/main/java/frc/robot/commands/ManualShootCommand.java +++ /dev/null @@ -1,48 +0,0 @@ -// 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.commands; - -import frc.robot.subsystems.TurretSubsystem; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj2.command.Command; - -/** An example command that uses an example subsystem. */ -public class ManualShootCommand extends Command { - private final TurretSubsystem m_subsystem; - private final XboxController m_controller; - - public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) { - m_subsystem = subsystem; - m_controller = controller; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if(m_controller.getRightTriggerAxis()>.5 && m_controller.getLeftTriggerAxis()==0) - { - m_subsystem.turretRotateDir(-.5); - m_controller.setRumble(RumbleType.kRightRumble, 1); - } - else if(m_controller.getLeftTriggerAxis()>.5 && m_controller.getRightTriggerAxis()==0) - { - m_subsystem.turretRotateDir(.5); - m_controller.setRumble(RumbleType.kLeftRumble, 1); - } - else - { - m_subsystem.turretRotateDir(0); - m_controller.setRumble(RumbleType.kBothRumble, 0); - } - } - - @Override - public boolean isFinished() { - return false; - } -} From 03586a46af05c8efbe7c5ec8ea50e4ff918a20f8 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 7 Apr 2026 20:24:56 -0400 Subject: [PATCH 24/27] Intake auto start --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a137a95..d505712 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -66,7 +66,7 @@ public class RobotContainer { * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight * joysticks}. - * + * */ private void configureBindings() { m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); @@ -92,7 +92,7 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Commands.parallel(intakeSubsystem.deploy(), Commands.run(() -> { + return Commands.parallel(Commands.parallel(intakeSubsystem.deploy(), intakeSubsystem.intake()), Commands.run(() -> { turretSubsystem.homeTurret(); }, turretSubsystem), driveSubsystem.getAutoCommand()); } From 56e2884210179405601ad1e8535eabada12788fe Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Tue, 7 Apr 2026 20:27:12 -0400 Subject: [PATCH 25/27] Nvm game management is bipolar and im undercaffinated --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d505712..286e2a0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -92,7 +92,7 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Commands.parallel(Commands.parallel(intakeSubsystem.deploy(), intakeSubsystem.intake()), Commands.run(() -> { + return Commands.parallel(intakeSubsystem.deploy(), Commands.run(() -> { turretSubsystem.homeTurret(); }, turretSubsystem), driveSubsystem.getAutoCommand()); } From 2ad0eedff5e3def679616174b590b359621ea314 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Thu, 9 Apr 2026 18:34:25 -0400 Subject: [PATCH 26/27] Wiggle Woggle Toggle Woggle --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/ZoneSwap.java | 144 ++++++++++++++++++ 2 files changed, 145 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/ZoneSwap.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 286e2a0..4e970a3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -82,7 +82,7 @@ public class RobotContainer { m_driverController.rightBumper().onTrue(intakeSubsystem.intake()); m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem)); m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); - m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } /** diff --git a/src/main/java/frc/robot/commands/ZoneSwap.java b/src/main/java/frc/robot/commands/ZoneSwap.java new file mode 100644 index 0000000..4f28ee1 --- /dev/null +++ b/src/main/java/frc/robot/commands/ZoneSwap.java @@ -0,0 +1,144 @@ +package frc.robot.commands; + +import java.util.Arrays; +import java.util.List; + +import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.Utils; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.TurretSubsystem; + +public class ZoneSwap extends Command { + + // For Chassis Control + private DriveSubsystem driveSubsystem; + + private HopperSubsystem hopperSubsystem; + private TurretSubsystem turretSubsystem; + + private PIDController cameraAutoCenterController; + private PIDController deadreckoningAutoCenterController; + + private int[] tagFilters; + + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + private Pose3d closestTag; + + private final double chassisRotateSpeed = Math.PI * 0.1; + + public ZoneSwap(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) { + this.driveSubsystem = driveSubsystem; + this.hopperSubsystem = hopperSubsystem; + this.turretSubsystem = turretSubsystem; + + this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem); + + cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0); + cameraAutoCenterController.setTolerance(0.2); // Units degrees + cameraAutoCenterController.setSetpoint(0); + + deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0); + deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations + deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void initialize() { + DriverStation.getAlliance().ifPresent((aliance) -> { + if (aliance == Alliance.Blue) { + tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; + } else { + tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; + } + }); + + turretSubsystem.setLauncher(6000); + turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); + hopperSubsystem.spindexerOn(); + cameraAutoCenterController.calculate(-100); + + // Change to HUB tracking pipeline + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER); + LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters); + + Pose3d botPose = driveSubsystem.getBotPose(); + // Find closest target to us + List tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList(); + closestTag = botPose.nearest(tagPoses); + Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closestTag); + + Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); + Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); + + deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians()); + } + + @Override + public void execute() { + // Need to compute shoot speed and angle + + Logger.recordOutput("AutoAlign-CenteringError", cameraAutoCenterController.getError()); + + // No april tag in camera view. Rotate towards our theoretical closest tag + if (!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)) { + Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning"); + + Pose3d botPose = driveSubsystem.getBotPose(); + //driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); + } else // We have an april tag, center it to the camera frame + { + Logger.recordOutput("AutoAlignState-Tracking", "Camera"); + // [tx, ty, tz, pitch, yaw, roll] + double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); + if (targetsPose.length >= 1) { + Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]); + + // Calculate turret Angle + double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO; + turretSubsystem.setTurretMotorRotation(-turretAngle); + + driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated + } + } + + // If robot is centered on HUB and shooter is up to speed. Run the kicker + if (turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()) { + Logger.recordOutput("AutoAlignState-Shoot", "FIRE!"); + hopperSubsystem.feedKicker(); + } else { + Logger.recordOutput("AutoAlignState-Shoot", "Waiting"); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + turretSubsystem.setLauncher(0); + + hopperSubsystem.stopKicker(); + hopperSubsystem.spindexerOff(); + + driveSubsystem.stopDrive(); + + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER); + } + +} From ad65dc07ceba2ccd1e7ff74f0a45f05e2cc0a2d7 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Thu, 9 Apr 2026 20:28:28 -0400 Subject: [PATCH 27/27] Zone check? I dunno, tired :3 --- src/main/java/frc/robot/Constants.java | 236 ++++++++++-------- src/main/java/frc/robot/RobotContainer.java | 2 + .../{ZoneSwap.java => AnyTarget.java} | 4 +- .../java/frc/robot/commands/ZoneCheck.java | 79 ++++++ .../frc/robot/subsystems/DriveSubsystem.java | 15 +- 5 files changed, 216 insertions(+), 120 deletions(-) rename src/main/java/frc/robot/commands/{ZoneSwap.java => AnyTarget.java} (97%) create mode 100644 src/main/java/frc/robot/commands/ZoneCheck.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e0b4832..4fdb60e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,151 +1,167 @@ // 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; +import com.ctre.phoenix6.signals.InvertedValue; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.units.Units; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.units.measure.Angle; -import com.ctre.phoenix6.signals.InvertedValue; - /** - * 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 - * globally (i.e. public static). Do not put anything functional in this class. + * 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 globally (i.e. public static). Do + * not put anything functional in this class. * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the constants are needed, to reduce verbosity. */ public final class Constants { - public static class OperatorConstants { - public static final int kDriverControllerPort = 0; - } - public static class CameraConstanst { + public static class OperatorConstants { - public static class IDFilters { - public static final int[] RED_HUB_SHOOTING_IDS = new int[]{ 8, 9, 10, 11 }; - public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 27, 26, 25, 24 }; + public static final int kDriverControllerPort = 0; } - public static final String BACK_CAMERA_NAME = "limelight-back"; - public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0; - public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1; - public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2; + public static class CameraConstanst { - public static final String FRONT_CAMERA_NAME = "limelight-front"; - public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0; - - } + public static class IDFilters { - public static class AdvantageKitConstants{ - public enum RobotType{ - SIM, - REAL, - REPLAY + public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11}; + public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24}; + } + + public static final String BACK_CAMERA_NAME = "limelight-back"; + public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0; + public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1; + public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2; + + public static final String FRONT_CAMERA_NAME = "limelight-front"; + public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0; + public static final double[] BLUE_ZONE_COOR = new double[]{1}; + public static final double[] RED_ZONE_COOR = new double[]{1}; + public static final double[] NEUTRAL_ZONE_COOR = new double[]{1}; } - public static final RobotType CURRENT_MODE = RobotType.REAL; - } - // Swerve offsets as of 3/2/26 - public static class SwerveDriveOffsets { - public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f; - public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f; - public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f; - public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f; - } - /* + + public static class AdvantageKitConstants { + + public enum RobotType { + SIM, + REAL, + REPLAY + } + public static final RobotType CURRENT_MODE = RobotType.REAL; + } + // Swerve offsets as of 3/2/26 + + public static class SwerveDriveOffsets { + + public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f; + public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f; + public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f; + public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f; + } + + /* * Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot * Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot * Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot * Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot * Laters numbers will be added for more subsystems * Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon - */ - public static class SwerveDriveIDs { - public static final int FRONT_LEFT_STEER_ID = 11; - public static final int FRONT_LEFT_DRIVE_ID = 12; - public static final int FRONT_LEFT_ENCODER_ID = 13; - public static final int FRONT_RIGHT_STEER_ID = 21; - public static final int FRONT_RIGHT_DRIVE_ID = 22; - public static final int FRONT_RIGHT_ENCODER_ID = 23; - public static final int BACK_LEFT_STEER_ID = 31; - public static final int BACK_LEFT_DRIVE_ID = 32; - public static final int BACK_LEFT_ENCODER_ID = 33; - public static final int BACK_RIGHT_STEER_ID = 41; - public static final int BACK_RIGHT_DRIVE_ID = 42; - public static final int BACK_RIGHT_ENCODER_ID = 43; - } + */ + public static class SwerveDriveIDs { - public static class RobotChassisLimits{ - // Max velocity in meters per second - public static final double MAX_ROBOT_LINEAR_VELOCITY = 3; - // In meters per second squared - public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2; + public static final int FRONT_LEFT_STEER_ID = 11; + public static final int FRONT_LEFT_DRIVE_ID = 12; + public static final int FRONT_LEFT_ENCODER_ID = 13; + public static final int FRONT_RIGHT_STEER_ID = 21; + public static final int FRONT_RIGHT_DRIVE_ID = 22; + public static final int FRONT_RIGHT_ENCODER_ID = 23; + public static final int BACK_LEFT_STEER_ID = 31; + public static final int BACK_LEFT_DRIVE_ID = 32; + public static final int BACK_LEFT_ENCODER_ID = 33; + public static final int BACK_RIGHT_STEER_ID = 41; + public static final int BACK_RIGHT_DRIVE_ID = 42; + public static final int BACK_RIGHT_ENCODER_ID = 43; + } - // In radians per second - public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI; - // In radians per second squared - public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI; - } + public static class RobotChassisLimits { + // Max velocity in meters per second - //Bird - public static class ExtraIDEntities { - public static final int PIGEON_ID = 1; - } - - public static class IntakeConstants { - public static final int DEPLOY_MOTOR_ID = 17; - public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this + public static final double MAX_ROBOT_LINEAR_VELOCITY = 3; + // In meters per second squared + public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2; - public static final int INTAKE_MOTOR_ID = 2; - public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this - } + // In radians per second + public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI; + // In radians per second squared + public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI; + } - public static class ShooterConstants { - public static final int LAUNCHER_MOTOR_ID = 3; - public static final boolean LAUNCHER_MOTOR_INVERTED = false; + //Bird + public static class ExtraIDEntities { - public static final int TURRET_MOTOR_ID = 4; - public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; - public static final int TURRET_LIMIT_CHANNEL = 9; - public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165); - public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); - public static final double HOMING_SPEED = .35; - public static final double TURRET_GEAR_RATIO = 6; + public static final int PIGEON_ID = 1; + } - public static final int HOOD_SERVO_CHANNEL = 0; - public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this - public static final int HOOD_ENCODER_ID = 4; - 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); - public static final double DEFLECTOR_STORED_ANGLE = 0; - public static final double DEFLECTOR_SET_ANGLE1 = 100; - public static final double DEFLECTOR_SET_ANGLE2 = 200; - public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed - public static final double DEFLECTOR_SERVO_RATIO = 1.0; - } - - public static class HopperConstants { - public static final int KICKER_MOTOR_ID = 9; - public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; + public static class IntakeConstants { - public static final int SPINDEXER_MOTOR_ID = 8; - public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; - } + public static final int DEPLOY_MOTOR_ID = 17; + public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this - public static class FieldPoses { - private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + public static final int INTAKE_MOTOR_ID = 2; + public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this + } - public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); - public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + public static class ShooterConstants { - } + public static final int LAUNCHER_MOTOR_ID = 3; + public static final boolean LAUNCHER_MOTOR_INVERTED = false; + + public static final int TURRET_MOTOR_ID = 4; + public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + public static final int TURRET_LIMIT_CHANNEL = 9; + public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165); + public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); + public static final double HOMING_SPEED = .35; + public static final double TURRET_GEAR_RATIO = 6; + + public static final int HOOD_SERVO_CHANNEL = 0; + public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this + public static final int HOOD_ENCODER_ID = 4; + 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); + public static final double DEFLECTOR_STORED_ANGLE = 0; + public static final double DEFLECTOR_SET_ANGLE1 = 100; + public static final double DEFLECTOR_SET_ANGLE2 = 200; + public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed + public static final double DEFLECTOR_SERVO_RATIO = 1.0; + } + + public static class HopperConstants { + + public static final int KICKER_MOTOR_ID = 9; + public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; + + public static final int SPINDEXER_MOTOR_ID = 8; + public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + } + + public static class FieldPoses { + + private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4e970a3..f441446 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import frc.robot.commands.AutoShootCommandChassisTurret; import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner; import frc.robot.commands.RelayCommand; import frc.robot.commands.SetDeflectorCommand; +import frc.robot.commands.ZoneCheck; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -83,6 +84,7 @@ public class RobotContainer { m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem)); m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + m_driverController.b().toggleOnTrue(new ZoneCheck()); } /** diff --git a/src/main/java/frc/robot/commands/ZoneSwap.java b/src/main/java/frc/robot/commands/AnyTarget.java similarity index 97% rename from src/main/java/frc/robot/commands/ZoneSwap.java rename to src/main/java/frc/robot/commands/AnyTarget.java index 4f28ee1..9604e33 100644 --- a/src/main/java/frc/robot/commands/ZoneSwap.java +++ b/src/main/java/frc/robot/commands/AnyTarget.java @@ -21,7 +21,7 @@ import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.TurretSubsystem; -public class ZoneSwap extends Command { +public class AnyTarget extends Command { // For Chassis Control private DriveSubsystem driveSubsystem; @@ -40,7 +40,7 @@ public class ZoneSwap extends Command { private final double chassisRotateSpeed = Math.PI * 0.1; - public ZoneSwap(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) { + public AnyTarget(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) { this.driveSubsystem = driveSubsystem; this.hopperSubsystem = hopperSubsystem; this.turretSubsystem = turretSubsystem; diff --git a/src/main/java/frc/robot/commands/ZoneCheck.java b/src/main/java/frc/robot/commands/ZoneCheck.java new file mode 100644 index 0000000..62f5359 --- /dev/null +++ b/src/main/java/frc/robot/commands/ZoneCheck.java @@ -0,0 +1,79 @@ +package frc.robot.commands; + +import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; + +public class ZoneCheck extends Command { + + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + private Zone zone; + + public enum Zone { + Blue, + Red, + Neutral; + } + + public ZoneCheck() { + getBotPose(); + } + + @Override + public void initialize() { + DriverStation.getAlliance().ifPresent((aliance) -> { + if (aliance == Alliance.Blue) { + switch (zone) { + case Blue: + System.out.println("Im Home"); + case Neutral: + System.out.println("Uhhhhhhhh not red"); + ; + default: + System.out.println("BOOOOO, BOOOO RED"); + ; + } + } else { + switch (zone) { + case Blue: + System.out.println("Screw Blue"); + ; + case Neutral: + System.out.println("Uhhhhhhhh not red"); + ; + default: + System.out.println("Im Home"); + } + } + }); + } + + @Override + public void execute() { + + } + + @Override + public boolean isFinished() { + return false; + } + + private double[] getBotPose() { + return LimelightHelpers.getBotPose_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); + } + + private void getZone() { + if (getBotPose()[0] >= Constants.CameraConstanst.RED_ZONE_COOR[0]) { + zone = Zone.Red; + } else if (getBotPose()[0] >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) { + zone = Zone.Neutral; + } else { + zone = Zone.Blue; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index d5066ca..85b5c8e 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -135,7 +135,6 @@ public class DriveSubsystem extends TestableSubsystem { .build()) .build(); - // Set vision measurement confidence values driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999)); @@ -166,7 +165,7 @@ public class DriveSubsystem extends TestableSubsystem { (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()), new PPHolonomicDriveController( new PIDConstants(1.5, 0, 0), - new PIDConstants(Math.PI/2, 0, 0)), + new PIDConstants(Math.PI / 2, 0, 0)), config, () -> { var alliance = DriverStation.getAlliance(); @@ -195,12 +194,12 @@ public class DriveSubsystem extends TestableSubsystem { Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d()); LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); - if(frontCameraPose.tagCount >= 1){ + if (frontCameraPose.tagCount >= 1) { driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds); Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose); } LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); - if(backCameraPose.tagCount >= 1){ + if (backCameraPose.tagCount >= 1) { driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); Logger.recordOutput("BackPoseEstimate", backCameraPose.pose); } @@ -212,15 +211,15 @@ public class DriveSubsystem extends TestableSubsystem { driveSystem.simulationPeriodic(); } - public Command getAutoCommand(){ + public Command getAutoCommand() { return autoChooser.getSelected(); } - public void stopDrive(){ - driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0,0,0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); + public void stopDrive() { + driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, 0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); } - public void rotateChassis(double rotSpeed){ + public void rotateChassis(double rotSpeed) { driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); }