From 7f1061ab9ab5b21edd30e1994f4145efb22c19de Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Sun, 20 Mar 2022 17:28:10 -0600
Subject: [PATCH] odo chooser stuff

---
 src/main/java/frc4388/robot/Robot.java               | 12 +++---------
 .../robot/commands/ShooterCommands/Shoot.java        |  6 ++----
 src/main/java/frc4388/robot/subsystems/Claws.java    |  4 ++--
 src/main/java/frc4388/robot/subsystems/Hood.java     |  2 +-
 4 files changed, 8 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 2232b4c..da81db4 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/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
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
index c4a448e..60c69e7 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+++ b/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);
         
diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java
index 2d8f726..2b811b0 100644
--- a/src/main/java/frc4388/robot/subsystems/Claws.java
+++ b/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);
diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java
index 29b09d2..7f81242 100644
--- a/src/main/java/frc4388/robot/subsystems/Hood.java
+++ b/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();