Turn Degrees command works, Keenan did work on Dead Assist Control Mode

src/main/java/frc4388/robot/Constants.java

@@ -30,8 +30,8 @@ public final class Constants {
         /* PID Constants Drive*/
         public static final int DRIVE_TIMEOUT_MS = 30;
         public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3);
-        public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.05);
-        public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.5);
+        public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.45);
+        public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.45);
         //public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
         //public static final int DRIVE_CRUISE_VELOCITY = 20000;
         //public static final int DRIVE_ACCELERATION = 7000;

src/main/java/frc4388/robot/RobotContainer.java

@@ -50,6 +50,7 @@ import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.commands.TrackTarget;
+import frc4388.robot.commands.TurnDegrees;
 import frc4388.robot.commands.Wait;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
@@ -144,7 +145,7 @@ public class RobotContainer {
       
         // resets the yaw of the pigeon
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive));
+            .whileHeld(new RunCommand(() -> m_robotDrive.runMotionMagicPID(12, 0), m_robotDrive));
       
         // turn 45 degrees
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
@@ -161,7 +162,7 @@ public class RobotContainer {
         
         // interrupts any running command
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
-            .whenPressed(new Wait(3, m_robotDrive));
+            .whenPressed(new TurnDegrees(90, m_robotDrive));
 
         // safety for climber and leveler
         new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)

src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpiutil.math.MathUtil;
 import frc4388.robot.Constants.DriveConstants;
@@ -21,6 +22,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   long m_deadTimeSteer, m_deadTimeMove;
   long m_deadTimeout = 100;
   IHandController m_controller;
+  boolean m_isInterrupted;
 
   /**
    * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
@@ -42,6 +44,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   @Override
   public void initialize() {
     m_currTime = System.currentTimeMillis();
+    resetGyroTarget();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -54,6 +57,11 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     m_deltaTime = System.currentTimeMillis() - m_currTime;
     m_currTime = System.currentTimeMillis();
 
+    if (m_isInterrupted) {
+      resetGyroTarget();
+      m_isInterrupted = false;
+    }
+
     /* If move stick is being used */
     if (moveInput != 0) {
       m_deadTimeMove = m_currTime;
@@ -90,16 +98,17 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     }
   }
 
-  private void runDriveWithInput(double move, double steer) {
+  private void runDriveWithInput(double move, double steerInput) {
     double cosMultiplier = .45;
     double steerOutput = 0;
     double deadzone = .2;
     /* Curves the steer output to be similarily gradual */
-    if (steer > 0){
-      steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
-    } else {
-      steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
+    if (steerInput > 0){
+      steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
+    } else if (steerInput < 0) {
+      steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
     }
+
     m_drive.driveWithInput(move, steerOutput);
     System.out.println("Driving With Input");
   }
@@ -110,8 +119,23 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   }
 
   private void runStoppedTurn(double steer) {
-    updateGyroTarget(steer);
-    m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
+    double cosMultiplier = 0.70;
+    double steerOutput = 0;
+    double deadzone = .2;
+    /* Curves the steer output to be similarily gradual */
+    if (steer > 0) {
+      steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
+    } else if (steer < 0) {
+      steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
+    }
+
+    updateGyroTarget(steerOutput);
+    double currentPos = m_drive.m_rightFrontMotorPos;
+    if (Math.abs(currentPos - m_stopPos) > 100) {
+      m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
+    } else {
+      m_drive.driveWithInputAux(0, m_targetGyro);
+    }
     System.out.println("Turning with Target: " + m_targetGyro);
   }
 
@@ -121,8 +145,8 @@ 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));
+                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
+                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
   }
 
   /**
@@ -137,6 +161,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    m_isInterrupted = interrupted;
   }
 
   // Returns true when the command should end.

src/main/java/frc4388/robot/commands/TurnDegrees.java

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+
+public class TurnDegrees extends CommandBase {
+
+  double m_targetAngle;
+  Drive m_drive;
+  double m_currentYawInTicks;
+  double m_targetAngleTicksIn;
+  double m_targetAngleTicksOut;
+  int i;
+
+  /**
+   * Creates a new TurnDeg.
+   */
+  public TurnDegrees(double targetAngle, Drive subsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    m_targetAngle = targetAngle;
+    m_drive = subsystem;
+
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks;
+
+    i = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+
+    m_drive.runTurningPID(m_targetAngleTicksOut);
+
+    SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
+    SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
+
+    i++;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
+      return true;
+    }
+    return false;
+  }
+}
+

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -469,9 +469,9 @@ public class Drive extends SubsystemBase {
    * @param targetAngle target angle in degrees
    */
   public void runTurningPID(double targetAngle) {
-    double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    //double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
 
-    runDriveVelocityPID(0, targetGyro);
+    runDriveVelocityPID(0, targetAngle);
   }
 
   /**