Attempting to tune turret

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

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

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());
         
+        if(turretSubsystem.isTurretReady()){
             turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).getMeasure());
+            // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(45, Units.Degrees));
+        }
 
     }
 

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() {
 

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)
+            .withKP(2)
-            .withKI(0)
+            .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;
+    turretState = TurretState.INIT;
-    limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG;
-    limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS;
-    turretGoingPos = false;
   }
 
   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()
+  public void homeTurret(){
-  {
+    turretState = TurretState.HOMING;
-    turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
+  }
-    currentState = 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);
-  }
 
   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:
+    if(turretState == TurretState.HOMING)
-    //     break;
+    {
-    // }
+      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)));
+    if(turretState != TurretState.READY)
-    turretSim.setRawRotorPosition(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
+    {
+      // Not ready to set angle
+      return;
     }
 
-  private void periodicManual()
+    if(pos.gt(Angle.ofRelativeUnits(90, Units.Degrees)) || pos.lt(Angle.ofRelativeUnits(-90, Units.Degrees)))
     {
-    if(passedLimit())
+      // Don't allow to over extend
-    {
+      return;
-      turretMotor.set(0);
     }
+
+    setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
+  }
+
+  public void setTurretMotorRotation(Angle pos){
+    turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset)));
+    turretSim.setRawRotorPosition(pos);
   }
 
   @PostTest()