FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/turret-auto-tracking'

This commit is contained in:
TheRealGrayden 2026-03-31 18:53:53 -04:00
parent 116a6505a2 2610f4fd77
commit 60693453e7
7 changed files with 234 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/frc/robot/Constants.java
View File

@ -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

5
src/main/java/frc/robot/RobotContainer.java
View File

@ -10,6 +10,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,10 @@ public class RobotContainer {
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
// turretSubsystem.homeTurret();
// Configure the trigger bindings
configureBindings();
}

5
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
View File

@ -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,6 +91,7 @@ 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());
}
@ -105,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())); // 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
{

94
src/main/java/frc/robot/commands/AutoTurretCommand.java Normal file
View File

@ -0,0 +1,94 @@
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.units.measure.Angle;
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.k2026RebuiltWelded);
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
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)));
} 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());
Logger.recordOutput("AutoTurret-Translation", targetTranslation);
if(turretSubsystem.isTurretReady()){
turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
// turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
}
}
@Override
public boolean isFinished() {
return false;
}
@Override
public void end(boolean interrupted) {
}
}

43
src/main/java/frc/robot/commands/ExampleCommand.java Normal file
View File

@ -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;
}
}

6
src/main/java/frc/robot/commands/ManualShootCommand.java
View File

@ -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);
}
}

155
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -11,12 +11,18 @@ 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;
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;
import com.revrobotics.spark.ClosedLoopSlot;
@ -28,6 +34,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;
@ -38,20 +46,24 @@ 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 StatusSignal<Angle> turretMotorPosition;
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
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,9 +74,10 @@ 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()
@ -72,8 +85,19 @@ public class TurretSubsystem extends TestableSubsystem {
)
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(50)
)
.withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(false).withReverseSoftLimitEnable(false))
.withFeedback(
new FeedbackConfigs()
.withFeedbackRotorOffset(0)
)
);
turretMotorPosition = turretMotor.getPosition();
turretMotorForwardLimit = turretMotor.getForwardLimit();
turretMotorReverseLimit = turretMotor.getReverseLimit();
turretSim = turretMotor.getSimState();
// Launcher configuration
launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless);
@ -92,10 +116,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) {
@ -141,95 +162,89 @@ public class TurretSubsystem extends TestableSubsystem {
public void startHoming()
{
turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
currentState = TurretState.HOMING;
turretState = TurretState.HOMING;
}
public void startRotate(double speed)
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().times(Constants.ShooterConstants.TURRET_GEAR_RATIO);
return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO);
}
private Angle getTurretMotorPosition(){
return turretMotorPosition.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 = turretMotorPosition.refresh().getValue().plus(Angle.ofRelativeUnits(100, 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);
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));
}
}
if(LOGGING)
{
Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-Launcher Volatge", launcherMotor.getOutputCurrent());
Logger.recordOutput("TurretSubsystem-TurretState", turretState);
Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().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());
}
}
private void periodicHoming()
public void setTurretRotation(Angle pos)
{
if(turretLimitSwitch.get())
if(turretState != TurretState.READY)
{
currentState = TurretState.MANUAL;
turretMotor.set(0);
turretMotor.setPosition(0);
goTo(limitPos);
// Not ready to set angle
return;
}
setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
}
private void goTo(Angle pos)
{
if(pos.gt(limitNeg) && pos.lt(limitPos))
{
while(getTurretAngle()!=pos)
{
if(getTurretAngle().gt(pos))
{
turretMotor.set(.5);
}
else
{
turretMotor.set(-.5);
}
}
}
}
private void periodicManual()
{
if(passedLimit())
{
turretMotor.set(0);
}
public void setTurretMotorRotation(Angle pos){
turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset)));
turretSim.setRawRotorPosition(pos.plus(turretOffset));
}
@PostTest()
public void TurretSubsystem_CANDevicesConnected(){
assertThat(launcherMotor.getDeviceId()).as("Launcher motor not connected!").isGreaterThan(0);
assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse();
assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue();
}