From 431be205e17b5f2d90f0cc1b7fb1b6ecc18333fe Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 27 Mar 2026 23:23:39 -0400 Subject: [PATCH 1/3] 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 2/3] 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 3/3] 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(); }