From 5dbe480995c39447a1df3223494260cada6d2af7 Mon Sep 17 00:00:00 2001
From: Shilab66 <89350258+Shilab66@users.noreply.github.com>
Date: Tue, 12 Dec 2023 17:57:42 -0500
Subject: [PATCH] setup field simulation

---
 src/main/kotlin/com/team4099/robot2023/Robot.kt              | 5 +++++
 src/main/kotlin/com/team4099/robot2023/RobotContainer.kt     | 3 +++
 .../team4099/robot2023/config/constants/FieldConstants.kt    | 4 ++--
 3 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt
index 5f8ec0d9..39adaf01 100644
--- a/src/main/kotlin/com/team4099/robot2023/Robot.kt
+++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt
@@ -5,6 +5,7 @@ import com.team4099.robot2023.auto.AutonomousSelector
 import com.team4099.robot2023.auto.PathStore
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.subsystems.falconspin.MotorChecker
 import com.team4099.robot2023.util.Alert
 import com.team4099.robot2023.util.Alert.AlertType
@@ -157,6 +158,7 @@ object Robot : LoggedRobot() {
   }
 
   override fun robotPeriodic() {
+
     //    RobotContainer.measurementsWithTimestamps.forEach {
     // RobotContainer.addVisionMeasurement(it) }
 
@@ -192,6 +194,9 @@ object Robot : LoggedRobot() {
     Logger.getInstance().recordOutput("LoggedRobot/port3", port3.voltage)
 
      */
+
+    Logger.getInstance().recordOutput("AprilTagPose/0", FieldConstants.homeAprilTags[0].pose.pose3d)
+    Logger.getInstance().recordOutput("AprilTagPose/1", FieldConstants.homeAprilTags[1].pose.pose3d)
   }
 
   override fun teleopInit() {
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index d2ef7602..842d7df1 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -7,6 +7,7 @@ import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
@@ -101,6 +102,8 @@ object RobotContainer {
     drivetrain.elevatorHeightSupplier = Supplier { superstructure.elevatorInputs.elevatorPosition }
     drivetrain.objectiveSupplier = Supplier { superstructure.objective }
     limelight.gamePieceToLookFor = { superstructure.objective }
+
+
   }
 
   fun mapDefaultCommands() {
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
index 30730ae1..e3c38d72 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
@@ -163,7 +163,6 @@ object FieldConstants {
   // church tags
   val homeAprilTags: List<AprilTag> =
     listOf(
-      AprilTag(1, Pose3d((40.45).inches, (108.19).inches, (45).centi.meters, Rotation3d())),
       AprilTag(
         0,
         Pose3d(
@@ -172,7 +171,8 @@ object FieldConstants {
           (46).centi.meters,
           Rotation3d()
         )
-      )
+      ),
+      AprilTag(1, Pose3d((40.45).inches, (108.19).inches, (45).centi.meters, Rotation3d()))
     )
 
   // AprilTag locations (do not flip for red alliance)