From 04618cdf7ffb49fb1c9a5a0f9b5823b4db5e9df0 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Sun, 6 Mar 2022 13:40:09 -0700
Subject: [PATCH] odometry sendable chooser (not tested)

---
 simgui.json                            | 13 ++++++++
 src/main/java/frc4388/robot/Robot.java | 41 ++++++++++++++++++++++++++
 2 files changed, 54 insertions(+)

diff --git a/simgui.json b/simgui.json
index 27dbfad..4e5f439 100644
--- a/simgui.json
+++ b/simgui.json
@@ -57,8 +57,21 @@
       "/LiveWindow/VisionOdometry": "Subsystem",
       "/SmartDashboard/Field": "Field2d",
       "/SmartDashboard/JVM Memory": "Command",
+      "/SmartDashboard/Odometry Chooser": "String Chooser",
       "/SmartDashboard/Scheduler": "Scheduler",
+      "/SmartDashboard/SendableChooser[0]": "String Chooser",
       "/SmartDashboard/Usable Deploy Space": "Command"
     }
+  },
+  "NetworkTables": {
+    "SmartDashboard": {
+      "Odometry Chooser": {
+        "open": true
+      },
+      "SendableChooser[0]": {
+        "open": true
+      },
+      "open": true
+    }
   }
 }
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 1e1ae82..3d533a6 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -7,15 +7,21 @@ package frc4388.robot;
 import java.io.File;
 import java.nio.file.Files;
 import java.nio.file.Path;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.Map;
 import java.util.logging.Level;
 import java.util.logging.Logger;
 import java.util.stream.IntStream;
 
 import com.diffplug.common.base.Errors;
 
+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.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -36,6 +42,10 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
+  private SendableChooser<Pose2d> odoChooser = new SendableChooser<Pose2d>();
+  private HashMap<String, Pose2d> odoChoices = new HashMap<>();
+  private Pose2d selectedOdo;
+
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
@@ -129,6 +139,13 @@ public class Robot extends TimedRobot {
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
 
+    // odo chooser stuff
+    addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), 
+                  new Pose2d(1, 2, new Rotation2d(Math.PI/3)),
+                  new Pose2d(1, 3, new Rotation2d(Math.PI/4)));
+    updateOdoChooser();
+    SmartDashboard.putData("Odometry Chooser", odoChooser);
+
     // print odometry data to smart dashboard for debugging (if causing timeout
     // errors, you can comment it)
     SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
@@ -136,6 +153,22 @@ public class Robot extends TimedRobot {
     SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
   }
 
+  public void updateOdoChooser() {
+    Pose2d currentPose = m_robotContainer.getOdometry();
+    odoChooser.setDefaultOption("(" + currentPose.getX() + ", " + currentPose.getY() + ", " + currentPose.getRotation().getDegrees() + ")", currentPose);
+    
+    for (Map.Entry<String,Pose2d> entry : odoChoices.entrySet()) {
+      odoChooser.addOption(entry.getKey(), entry.getValue());
+    }
+  }
+
+  public void addOdoChoices(Pose2d... points) {
+    for (Pose2d point : points) {
+      String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + ")";
+      odoChoices.put(key, point);
+    }
+  }
+
   /**
    * This function is called once each time the robot enters Disabled mode.
    * You can use it to reset any subsystem information you want to clear when
@@ -168,6 +201,10 @@ public class Robot extends TimedRobot {
   @Override
   public void autonomousInit() {
     LOGGER.fine("autonomousInit()");
+
+    selectedOdo = odoChooser.getSelected();
+    m_robotContainer.resetOdometry(selectedOdo);
+
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
@@ -188,6 +225,10 @@ public class Robot extends TimedRobot {
   public void teleopInit() {
     LOGGER.fine("teleopInit()");
     m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
+
+    selectedOdo = odoChooser.getSelected();
+    m_robotContainer.resetOdometry(selectedOdo);
+
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove