setup field simulation

team4099 · Dec 12, 2023 · 5dbe480 · 5dbe480
1 parent 8080bfa
commit 5dbe480
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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)