Auto Turret Tracking is working in the simulator. Just no limits are set

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

@@ -12,6 +12,7 @@ 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.RelayCommand;
 import frc.robot.commands.SetDeflectorCommand;
 import frc.robot.commands.ZoneCheck;
@@ -56,6 +57,7 @@ public class RobotContainer {
 
         NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
 
+        turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
     }
 
     /**
@@ -84,7 +86,6 @@ 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(driveSubsystem));
     }
 
     /**

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

@@ -68,10 +68,10 @@ public class AutoTurretCommand extends Command {
         Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
         Logger.recordOutput("AutoTurret-Translation", targetTranslation);
 
-
         if(turretSubsystem.isTurretReady()){
-            // turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
+            turretSubsystem.setTurretFieldRelative(botPose, targetTranslation.getAngle().getRotations());
-            // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
+            // turretSubsystem.setTurretRobotRelative(0.25);
+            // turretSubsystem.setTurretMotorRotation(0);
         }
 
     }

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

@@ -34,6 +34,9 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 import frc.robot.Constants;
+import frc.robot.Robot;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.AngleUnit;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
@@ -58,6 +61,9 @@ public class TurretSubsystem extends TestableSubsystem {
   private StatusSignal<Angle> turretMotorPosition;
   private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
   private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
+  private double forwardLimit = 0;
+  private double reverseLimit = 0;
+
   private SparkMax launcherMotor;
   private SparkClosedLoopController launcherControler;
 
@@ -192,20 +198,23 @@ public class TurretSubsystem extends TestableSubsystem {
       { // Turret has tripped the switch
         turretMotor.stopMotor();
         turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
+        if(!Robot.isReal()){
+          turretOffsetInRotations = 1.5; // Flip turret around
+        }
         turretState = TurretState.READY;
-        // Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset);
+        forwardLimit = (1.5 + turretOffsetInRotations);
-        // Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset);
+        reverseLimit = (-1.5 + turretOffsetInRotations);
-        // Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
+        Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
-        // Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
+        Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
         turretMotor.getConfigurator().apply(
           new SoftwareLimitSwitchConfigs()
-            // .withForwardSoftLimitEnable(true)
+            .withForwardSoftLimitEnable(true)
-            // .withForwardSoftLimitThreshold(forwardLimit)
+            .withForwardSoftLimitThreshold(forwardLimit)
-            // .withReverseSoftLimitEnable(true)
+            .withReverseSoftLimitEnable(true)
-            // .withReverseSoftLimitThreshold(reverseLimit)
+            .withReverseSoftLimitThreshold(reverseLimit)
         );
-        // setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees));
+        setTurretRobotRelative(0);
-        setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
+        // setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
       }
     }
 
@@ -214,10 +223,10 @@ 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());
+      Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative());
       Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
-      Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
+      Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Rotations));
-      Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees));
+      Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Rotations));
       Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
       Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
       Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
@@ -226,13 +235,21 @@ public class TurretSubsystem extends TestableSubsystem {
 
   }
 
-  public void setTurretFieldRelative(double pos)
+  public void setTurretFieldRelative(Pose3d robotPose, double pos)
   {
     if(turretState != TurretState.READY)
     {
       // Not ready to set angle
       return;
     }
+
+    double correctedPos = -pos; // Flip angle to match CCW positive. Add 0.5 to flip around
+
+    double robotRotations = robotPose.getRotation().toRotation2d().getRotations();
+
+    Angle turretAngle = Angle.ofRelativeUnits(robotRotations, Units.Rotations).plus(Angle.ofRelativeUnits(correctedPos, Units.Rotations));
+
+    setTurretMotorRotation((turretAngle.in(Units.Rotation) * Constants.ShooterConstants.TURRET_GEAR_RATIO));
   }
 
   public void setTurretRobotRelative(double pos)
@@ -243,12 +260,32 @@ public class TurretSubsystem extends TestableSubsystem {
       return;
     }
 
-    setTurretMotorRotation(pos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
+    double correctedPos = -pos; // Flip angle to match CCW positive
+
+    setTurretMotorRotation(correctedPos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
   }
 
   public void setTurretMotorRotation(double pos){
-    turretMotor.setControl(turretControlMode.withPosition((pos + turretOffsetInRotations)));
+    double posWithOffset = pos + turretOffsetInRotations;
-    turretSim.setRawRotorPosition(pos + turretOffsetInRotations);
+
+    Logger.recordOutput("Blah", posWithOffset);
+
+    if(Robot.isSimulation()){
+      if(posWithOffset >= forwardLimit){
+        turretSim.setForwardLimit(true);
+      } else {
+        turretSim.setForwardLimit(false);
+      }
+
+      if(posWithOffset <= reverseLimit){
+        turretSim.setReverseLimit(true);
+      } else {
+        turretSim.setReverseLimit(false);
+      }
+    }
+
+    turretMotor.setControl(turretControlMode.withPosition(posWithOffset));
+    turretSim.setRawRotorPosition(posWithOffset);
   }
 
   private double getRequestedPosTurretRelative() {