From 9c2a897236c75603ca5ff43ea16e589b9a05b727 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 19 Feb 2020 09:14:03 -0700 Subject: [PATCH] Cleanup DeadAssist --- .../DriveWithJoystickUsingDeadAssistPID.java | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 7eff284..3ce828f 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -112,16 +112,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { private void runStoppedTurn(double steer) { updateGyroTarget(steer); m_drive.runDrivePositionPID(m_stopPos, m_targetGyro); - System.out.println("Turning with Target: " + m_targetGyro); - - /* if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity() > 3000) { - m_drive.driveWithInputAux(0, m_targetGyro); - System.out.println("!Turning with Target: " + m_targetGyro); - } else { - m_drive.runDriveVelocityPID(0, m_targetGyro); - System.out.println("Turning with Target: " + m_targetGyro); - }*/ } /** @@ -129,7 +120,6 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { */ private void updateGyroTarget(double steerInput) { m_targetGyro -= 5 * steerInput * m_deltaTime; - m_targetGyro = MathUtil.clamp( m_targetGyro, m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8), m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8)); @@ -142,6 +132,10 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { m_targetGyro = m_currentGyro; m_targetGyro = m_currentGyro + (3 * m_drive.m_rightFrontMotor.getSelectedSensorVelocity(1)); + + m_targetGyro = MathUtil.clamp( m_targetGyro, + m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8), + m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8)); } // Called once the command ends or is interrupted.