odo chooser stuff

src/main/java/frc4388/robot/Robot.java

@@ -170,9 +170,7 @@ public class Robot extends TimedRobot {
     // SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
     // SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
     // 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)));
+    addOdoChoices(new Pose2d(3.2766, -0.9398, new Rotation2d(0)));
     updateOdoChooser();
     SmartDashboard.putData("Odometry Chooser", odoChooser);
   }
@@ -185,7 +183,7 @@ public class Robot extends TimedRobot {
 
   public void addOdoChoices(Pose2d... points) {
     for (Pose2d point : points) {
-      String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + "°)";
+      String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + ")";
       odoChoices.put(key, point);
     }
   }
@@ -253,17 +251,13 @@ public class Robot extends TimedRobot {
   public void teleopInit() {
     LOGGER.fine("teleopInit()");
 
-
     Robot.alliance = DriverStation.getAlliance();
 
-    // m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
-
     selectedOdo = odoChooser.getSelected();
     if (selectedOdo == null) {
       selectedOdo = m_robotContainer.getOdometry();
     }
-    // m_robotContainer.resetOdometry(selectedOdo);
-
+    m_robotContainer.resetOdometry(selectedOdo);
 
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to

src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java

@@ -121,10 +121,8 @@ public class Shoot extends CommandBase {
         timerStarted = false;
         startTime = 0;
         
-        // this.turret.gotoMidpoint();
-        
-        this.odoX = 0;//-m_swerve.getOdometry().getY();
-        this.odoY = -8;//-m_swerve.getOdometry().getX();
+        this.odoX = 0.9398;//-this.swerve.getOdometry().getY(); // 3.2766
+        this.odoY = -3.2766;//-this.swerve.getOdometry().getX(); // 0.9398
         
         this.distance = Math.hypot(odoX, odoY);
         

src/main/java/frc4388/robot/subsystems/Claws.java

@@ -153,8 +153,8 @@ public class Claws extends SubsystemBase {
 
   // @Override
   public void periodic() {
-    SmartDashboard.putNumber("Servo Left Pos", m_leftClaw.getRaw());
-    SmartDashboard.putNumber("Servo Right Pos", m_rightClaw.getRaw());
+    // SmartDashboard.putNumber("Servo Left Pos", m_leftClaw.getRaw());
+    // SmartDashboard.putNumber("Servo Right Pos", m_rightClaw.getRaw());
   //   if (fullyOpen() || fullyClosed()) {
   //     m_leftClaw.setSpeed(0.0);
   //     m_rightClaw.setSpeed(0.0);

src/main/java/frc4388/robot/subsystems/Hood.java

@@ -61,7 +61,7 @@ public class Hood extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition());
+    // SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition());
 
     // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
     runVelocityRamping();