Goated changes sim

Author: MqxS

choreo.chor

@@ -104945,6 +104945,41 @@
             "circleObstacles": [],
             "eventMarkers": [],
             "isTrajectoryStale": false
+        },
+        "New Path": {
+            "waypoints": [
+                {
+                    "x": 8.274157524108887,
+                    "y": 7.469854831695557,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "trajectoryWaypoints": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [],
+            "eventMarkers": [],
+            "isTrajectoryStale": true
         }
     },
     "splitTrajectoriesAtStopPoints": true,

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

@@ -1,8 +1,6 @@
 package frc.robot;
 
 import com.ctre.phoenix6.SignalLogger;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.PowerDistribution;
@@ -13,7 +11,6 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.auto.AutoChooser;
 import frc.robot.auto.AutoOption;
 import frc.robot.auto.Autos;

src/main/java/frc/robot/auto/Autos.java

@@ -5,7 +5,6 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
@@ -21,7 +20,6 @@
 import frc.robot.subsystems.superstructure.ShotParameters;
 import frc.robot.subsystems.superstructure.Superstructure;
 import frc.robot.subsystems.vision.PhotonVision;
-import org.littletonrobotics.junction.Logger;
 
 import java.util.List;
 import java.util.Set;

src/main/java/frc/robot/constants/Constants.java

@@ -10,7 +10,7 @@
 import org.photonvision.PhotonPoseEstimator;
 
 public interface Constants {
-    RobotMode CURRENT_MODE = RobotMode.REAL;
+    RobotMode CURRENT_MODE = RobotMode.SIM;
     CompetitionType CURRENT_COMPETITION_TYPE = CompetitionType.TESTING;
     double LOOP_PERIOD_SECONDS = 0.02;
 
@@ -40,22 +40,21 @@ interface Vision {
                 new Translation3d(Units.inchesToMeters(11.862), Units.inchesToMeters(12.681), Units.inchesToMeters(8.947)),
                 new Rotation3d(0, Units.degreesToRadians(-25), Units.degreesToRadians(20))
         );
-
         Transform3d ROBOT_TO_FC_APRILTAG = new Transform3d(
                 new Translation3d(Units.inchesToMeters(13.36613), Units.inchesToMeters(8.5202), Units.inchesToMeters(6.759)),
                 new Rotation3d(0, Units.degreesToRadians(-30), 0)
         );
-
         Transform3d ROBOT_TO_FR_APRILTAG = new Transform3d(
                 new Translation3d(Units.inchesToMeters(11.838), Units.inchesToMeters(-12.861), Units.inchesToMeters(8.947)),
                 new Rotation3d(0, Units.degreesToRadians(-25), Units.degreesToRadians(-20))
         );
-
-        Transform3d ROBOT_TO_REAR_NOTED = new Transform3d(
+        Transform3d ROBOT_TO_REAR_NOTE = new Transform3d(
                 new Translation3d(Units.inchesToMeters(-16), Units.inchesToMeters(0), Units.inchesToMeters(14)),
                 new Rotation3d(0, Units.degreesToRadians(21), Units.degreesToRadians(180))
         );
 
+        double NOTE_HEIGHT_Z = Units.inchesToMeters(2);
+
         /**
          * Standard deviations of the supplied pose estimate (before vision, likely to be solely wheel odometry)
          */

src/main/java/frc/robot/constants/FieldConstants.java

@@ -36,6 +36,14 @@ public class FieldConstants {
     public static final Pose2d RED_AMP_POSE = PoseUtils.flip(BLUE_AMP_POSE);
     public static final Pose2d RED_AMP_SCORING_POSE = PoseUtils.flip(BLUE_AMP_SCORING_POSE);
 
+    public static final Pose2d[] CENTER_LINE_NOTE_POSES = new Pose2d[] {
+            new Pose2d(FIELD_LENGTH_X_METERS/2, 7.47, Rotation2d.fromDegrees(0)),
+            new Pose2d(FIELD_LENGTH_X_METERS/2, 5.79, Rotation2d.fromDegrees(0)),
+            new Pose2d(FIELD_LENGTH_X_METERS/2, FIELD_WIDTH_Y_METERS / 2.0, Rotation2d.fromDegrees(0)),
+            new Pose2d(FIELD_LENGTH_X_METERS/2, FIELD_WIDTH_Y_METERS - 5.79, Rotation2d.fromDegrees(0)),
+            new Pose2d(FIELD_LENGTH_X_METERS/2, FIELD_WIDTH_Y_METERS - 7.47, Rotation2d.fromDegrees(0)),
+    };
+
     private static Pose2d getAllianceFlippedPose(final Pose2d blueAlliancePose, final Pose2d redAlliancePose) {
         final Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
         if (alliance.isPresent()) {

src/main/java/frc/robot/constants/SimConstants.java

@@ -5,6 +5,7 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import frc.robot.subsystems.drive.constants.SwerveConstants;
+import org.photonvision.estimation.TargetModel;
 
 public interface SimConstants {
     // Assume 2mOhm resistance for voltage drop calculation
@@ -41,4 +42,9 @@ interface SwerveModules {
         /** Simulated steer voltage required to overcome friction. */
         double STEER_KS_VOLTS = 0.25;
     }
+
+    interface Vision {
+        TargetModel NOTE_TARGET_MODEL =
+                new TargetModel(Units.inchesToMeters(14), Units.inchesToMeters(14), Units.inchesToMeters(2));
+    }
 }

src/main/java/frc/robot/subsystems/drive/Swerve.java

@@ -13,7 +13,6 @@
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -34,6 +33,7 @@
 import frc.robot.constants.Constants;
 import frc.robot.constants.HardwareConstants;
 import frc.robot.subsystems.drive.constants.SwerveConstants;
+import frc.robot.subsystems.drive.estimator.SwerveDrivePoseEstimator;
 import frc.robot.subsystems.drive.trajectory.HolonomicChoreoController;
 import frc.robot.subsystems.drive.trajectory.HolonomicDriveWithPIDController;
 import frc.robot.subsystems.gyro.Gyro;
@@ -360,6 +360,10 @@ public Pose2d getPose() {
         return poseEstimator.getEstimatedPosition();
     }
 
+    public Optional<Pose2d> getPose(final double atTimestamp) {
+        return poseEstimator.sampleAt(atTimestamp);
+    }
+
     public Gyro getGyro() {
         return gyro;
     }