From 03daeed5060ed95f60114ffbfa51ba9cd2969834 Mon Sep 17 00:00:00 2001
From: Aarav Shah <91212@psdschools.org>
Date: Thu, 13 Feb 2020 19:35:30 -0700
Subject: [PATCH] Fixed heading in odometry, but position needs work

---
 src/main/java/frc4388/robot/Robot.java            | 1 +
 src/main/java/frc4388/robot/subsystems/Drive.java | 5 +++--
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index ca009f0..1335ec6 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -89,6 +89,7 @@ public class Robot extends TimedRobot {
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
+      System.err.println("Auto Start");
     }
   }
 
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 7a153e4..b3afa0d 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -77,7 +77,7 @@ public class Drive extends SubsystemBase {
     m_pigeon.configFactoryDefault();
     resetGyroYaw();
 
-    m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
+    m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()), new Pose2d(0, 0, new Rotation2d()));
 
     speedShift = new DoubleSolenoid(7,0,1);
 
@@ -263,6 +263,7 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
 
       SmartDashboard.putString("Odometry Values Meters", getPose().toString());
+      SmartDashboard.putNumber("Odometry Heading", getHeading());
 
     } catch (Exception e) {
       System.err.println("Error in the Drive Subsystem");
@@ -271,7 +272,7 @@ public class Drive extends SubsystemBase {
 
     m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               inchesToMeters(getDistanceInches(m_leftFrontMotor)),
-                                              inchesToMeters(getDistanceInches(m_rightFrontMotor)));
+                                              inchesToMeters(-getDistanceInches(m_rightFrontMotor)));
   }
 
   /**