Add Simulation visualization and Fix turret locking

simgui-ds.json

@@ -99,10 +99,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
-    },
-    {
+      "guid": "030000004c050000e60c000000000000",
       "useGamepad": true
     }
   ]

src/main/java/frc/robot/Robot.java

@@ -47,6 +47,7 @@ public void robotPeriodic() {
     // Logging to SmartDashboard
     // RobotContainer.logPDHValues();
     RobotContainer.AddVisionMeasurement().schedule();
+    RobotContainer.updateLoggedPoses();
     SmartDashboard.putBoolean("Is Practice Bot", RobotContainer.isPracticeBot());
     SmartDashboard.putString("Current Locked Location", RobotContainer.getLockedLocation().toString());
   }

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

@@ -10,6 +10,7 @@
 import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -25,7 +26,6 @@
 import frc.robot.Constants.LockedLocation;
 import frc.robot.Constants.constLEDs;
 import frc.robot.RobotMap.mapControllers;
-import frc.robot.RobotPreferences.prefIntake;
 import frc.robot.RobotPreferences.prefPitch;
 import frc.robot.RobotPreferences.prefVision;
 import frc.robot.RobotPreferences.prefShooter;
@@ -88,6 +88,14 @@ public class RobotContainer implements Logged {
   int[] XTranslationColor;
   int[] YTranslationColor;
 
+  // Poses
+  @Log.NT
+  static Pose3d currentRobotPose;
+  @Log.NT
+  static Pose3d turretPose = new Pose3d();
+  @Log.NT
+  static Pose3d hoodPose = new Pose3d();
+
   public RobotContainer() {
     conDriver.setLeftDeadband(constControllers.DRIVER_LEFT_STICK_DEADBAND);
 
@@ -256,6 +264,12 @@ public static boolean isPracticeBot() {
     return !isPracticeBot.get();
   }
 
+  public static void updateLoggedPoses() {
+    currentRobotPose = subDrivetrain.getPose3d();
+    turretPose = subTurret.getAngleAsPose3d();
+    hoodPose = turretPose.plus(subPitch.getAngleAsTransform3d());
+  }
+
   // --- PDH ---
 
   /**

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

@@ -48,13 +48,13 @@ public LockTurret(Turret subTurret, Drivetrain subDrivetrain, Climber subClimber
   @Override
   public void initialize() {
     desiredAngle = Rotation2d.fromDegrees(subTurret.getAngle());
-
-    fieldPoses = FieldConstants.GET_FIELD_POSITIONS();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    // This is in initialize in case the alliance changes after command init
+    fieldPoses = FieldConstants.GET_FIELD_POSITIONS();
     robotPose = subDrivetrain.getPose();
 
     Optional<Rotation2d> calculatedAngle = subTurret.getDesiredAngleToLock(robotPose, fieldPoses,

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

@@ -38,11 +38,9 @@ public class Drivetrain extends SN_SuperSwerve implements Logged {
   // Struct logging - Allows for logging data that SmartDashboard alone can't log,
   // but must be called on the variable's creation
   @Log.NT
-  private SwerveModuleState[] loggedDesiredStates;
+  private static SwerveModuleState[] loggedDesiredStates;
   @Log.NT
-  private SwerveModuleState[] loggedActualStates;
-  @Log.NT
-  private Pose3d currentRobotPose;
+  private static SwerveModuleState[] loggedActualStates;
 
   private static SN_SwerveModule[] modules = new SN_SwerveModule[] {
       new SN_SwerveModule(0, mapDrivetrain.FRONT_LEFT_DRIVE_CAN, mapDrivetrain.FRONT_LEFT_STEER_CAN,
@@ -163,7 +161,6 @@ public void setDefenseMode() {
   public void updateMonologueValues() {
     loggedDesiredStates = getDesiredModuleStates();
     loggedActualStates = getActualModuleStates();
-    currentRobotPose = new Pose3d(getPose());
   }
 
   /**
@@ -193,6 +190,10 @@ public double getVelocityToSnap(Rotation2d desiredYaw) {
     return yawSetpoint;
   }
 
+  public Pose3d getPose3d() {
+    return new Pose3d(getPose());
+  }
+
   @Override
   public void periodic() {
     super.periodic();

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

@@ -6,9 +6,7 @@
 
 import java.util.Optional;
 
-import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.controls.PositionVoltage;
@@ -18,14 +16,14 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.LockedLocation;
 import frc.robot.Constants.constPitch;
-import frc.robot.Constants.constTurret;
 import frc.robot.RobotContainer;
 import frc.robot.RobotMap.mapPitch;
 import frc.robot.RobotPreferences.prefPitch;
@@ -102,7 +100,8 @@ public void setPitchSoftwareLimits(boolean reverse, boolean forward) {
   }
 
   /**
-   * Sets the angle of the pitch motor
+   * Sets the angle of the pitch motor. The angle will not be set if the angle is
+   * not possible.
    * 
    * @param angle        The angle to set the pitch motor to. <b> Units: </b>
    *                     Degrees
@@ -113,8 +112,10 @@ public void setPitchAngle(double angle, boolean hasCollision) {
     if (hasCollision && angle >= prefPitch.pitchMaxIntake.getValue()) {
       angle = (angle >= prefPitch.pitchMaxIntake.getValue()) ? prefPitch.pitchMaxIntake.getValue() : getPitchAngle();
     }
-    desiredPitchAngle = angle;
-    pitchMotor.setControl(motionMagicRequest.withPosition(Units.degreesToRotations(angle)));
+    if (isAnglePossible(angle)) {
+      desiredPitchAngle = angle;
+      pitchMotor.setControl(motionMagicRequest.withPosition(Units.degreesToRotations(angle)));
+    }
   }
 
   public void setPitchGoalAngle(double angle) {
@@ -241,6 +242,11 @@ public Optional<Rotation2d> getDesiredAngleToLock(Pose2d robotPose, Pose3d[] fie
     return Optional.of(desiredLockingAngle);
   }
 
+  public Transform3d getAngleAsTransform3d() {
+    return new Transform3d(new Translation3d(),
+        new Rotation3d(0, -Units.degreesToRadians(desiredPitchAngle), 0));
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

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

@@ -4,7 +4,6 @@
 
 package frc.robot.subsystems;
 
-import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
 import com.ctre.phoenix6.controls.NeutralOut;

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

@@ -6,7 +6,6 @@
 
 import java.util.Optional;
 
-import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.NeutralOut;
@@ -17,7 +16,9 @@
 import edu.wpi.first.math.geometry.Pose2d;
 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.Translation3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -142,8 +143,16 @@ public void setTurretSpeed(double speed) {
     turretMotor.set(speed);
   }
 
+  /**
+   * Sets the desired goal angle of the turret. It will check if the given angle
+   * is possible before setting it.
+   * 
+   * @param angle The angle, in degrees
+   */
   public void setTurretGoalAngle(double angle) {
-    desiredTurretAngle = angle;
+    if (isAnglePossible(angle)) {
+      desiredTurretAngle = angle;
+    }
   }
 
   public double getTurretCurrent() {
@@ -263,6 +272,10 @@ public Optional<Rotation2d> getDesiredAngleToLock(Pose2d robotPose, Pose3d[] fie
     // Get the angle of 0,0 to the turret pose
     desiredLockingAngle = new Rotation2d(relativeToTarget.getX(), relativeToTarget.getY());
 
+    // Account for robot rotation
+    desiredLockingAngle = desiredLockingAngle
+        .rotateBy(robotPose.getRotation().minus(new Rotation2d().fromDegrees(180)));
+
     return Optional.of(desiredLockingAngle);
   }
 
@@ -275,6 +288,11 @@ public boolean isAnglePossible(double angle) {
         && angle >= Units.rotationsToDegrees(prefTurret.turretReverseLimit.getValue()));
   }
 
+  public Pose3d getAngleAsPose3d() {
+    return new Pose3d(new Translation3d(),
+        new Rotation3d(0, 0, Units.degreesToRadians(desiredTurretAngle)));
+  }
+
   @Override
   public void periodic() {
     SmartDashboard.putNumber("Turret/Absolute Encoder Raw Value (Rotations)", getRawAbsoluteEncoder());