From 51f2ad4c30361a94a1b30354f4edb47df3d97a54 Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Sun, 10 Mar 2024 14:01:47 -0400
Subject: [PATCH] Drivetrain teleop drive and basic auto seems to be ok now

---
 src/main/java/frc/robot/RobotContainer.java        |  2 +-
 src/main/java/frc/robot/subsystems/Drivetrain.java | 12 +++++++++++-
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1cb3e44..5fafcc4 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -157,7 +157,7 @@ public class RobotContainer {
         );
 
         //intake.setDefaultCommand(intake.intakeUpCommand());
-        intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, operator::getRightY));
+        intake.setDefaultCommand(intake.manualPivot(() -> -operator.getLeftY(), operator::getRightY));
 
         shooter.setDefaultCommand(
             shooter.angleSpeedsSetpoints(
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 0975eed..a087c3c 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -420,7 +420,17 @@ public class Drivetrain extends SubsystemBase {
 
     /** Zeroes the heading of the robot. */
     public void zeroHeading() {
-        m_gyro.reset();
+        Pose2d tempPose = m_poseEstimator.getEstimatedPosition();
+
+        Rotation2d tempRotation = Rotation2d.fromDegrees(0);
+
+        resetOdometry(
+            new Pose2d(
+                tempPose.getX(), 
+                tempPose.getY(), 
+                tempRotation
+            )
+        );
     }
 
     public Command zeroHeadingCommand() {