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

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

src/main/java/frc/robot/RobotContainer.java

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

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,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
         {

src/main/java/frc/robot/commands/AutoTurretCommand.java

@@ -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) {
+    }
+
+}

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

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

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -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)
+            .withKP(2)
-            .withKI(0)
+            .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;
+    turretState = TurretState.INIT;
-    limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG;
-    limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS;
-    turretGoingPos = false;
   }
 
   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;
-    turretGoingPos=speed>0;
+  }
 
-    if(!passedLimit())
+  public void turretRotateDir(double speed)
   {
     turretMotor.set(speed);
   }
-    else
-    {
-      turretMotor.set(0);
-    }
-  }
-
-  public boolean passedLimit(){
-    return (getTurretAngle().gt(limitPos) && turretGoingPos) || (getTurretAngle().lt(limitNeg) && !turretGoingPos);
-  }
 
   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:
+    if(turretState == TurretState.HOMING)
-        break;
+    {
+      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;
+      // Not ready to set angle
-      turretMotor.set(0);
+      return;
-      turretMotor.setPosition(0);
-      goTo(limitPos);
-    }
     }
 
-  private void goTo(Angle pos)
+    setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
-  {
-    if(pos.gt(limitNeg) && pos.lt(limitPos))
-    {
-      while(getTurretAngle()!=pos)
-    {
-      if(getTurretAngle().gt(pos))
-      {
-        turretMotor.set(.5);
-      }
-      else
-      {
-        turretMotor.set(-.5);
-      }
-    }
-    }
   }
 
-  private void periodicManual()
+  public void setTurretMotorRotation(Angle pos){
-  {   
+    turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset)));
-    if(passedLimit())
+    turretSim.setRawRotorPosition(pos.plus(turretOffset));
-    {
-      turretMotor.set(0);
-    }
   }
 
   @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();
   }