From 6cc1f7fb50a79df2096d4c61cef7f0c5d7f82f0d Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 18 Feb 2020 19:44:11 -0700
Subject: [PATCH] Messing with Dead Assist

---
 .../DriveWithJoystickUsingDeadAssistPID.java         | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
index 74b662e..7eff284 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
@@ -77,6 +77,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
       /* If steer stick has not been used for less than 1 sec */
       if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
         runDriveWithInput(moveOutput, steerInput);
+        resetGyroTarget();
       }
       /* If steer stick has not been used for 1 sec */
       else {
@@ -99,7 +100,6 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     } else {
       steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
     }
-    resetGyroTarget();
     m_drive.driveWithInput(move, steerOutput);
     System.out.println("Driving With Input");
   }
@@ -131,17 +131,17 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     m_targetGyro -= 5 * steerInput * m_deltaTime;
 
     m_targetGyro = MathUtil.clamp(  m_targetGyro,
-                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
-                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
+                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8),
+                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8));
   }
 
   /**
    * set target angle to current angle (prevents buildup of gyro error).
    */
   private void resetGyroTarget() {
-    //m_targetGyro = m_currentGyro;
-    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1)
-                      + (3 * m_drive.m_rightFrontMotor.getSelectedSensorVelocity(1));
+    m_targetGyro = m_currentGyro;
+    m_targetGyro =  m_currentGyro
+                    + (3 * m_drive.m_rightFrontMotor.getSelectedSensorVelocity(1));
   }
 
   // Called once the command ends or is interrupted.