Sort Commands and Organise Imports

src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java

@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+
+public class DrivePositionMPAux extends CommandBase {
+  Drive m_drive;
+  double m_cruiseVel;
+  double m_rampDist;
+  double m_targetPos;
+  double m_currentVel;
+  double m_currentPos;
+  double m_targetGyro;
+  double m_targetVel;
+  double m_rampAcc;
+  long m_startTime;
+  long m_rampRate;
+  int m_counter;
+
+  /**
+   * Creates a new DrivePositionMPAux.
+   * 
+   * @param subsystem The drive subsystem
+   * @param cruiseVel The target velocity for the motors in in/s
+   * @param rampDist  The distance before cruise velocity is reached in inches
+   * @param rampRate  The time to reach the cruise velocity in seconds
+   * @param targetPos The target position
+   */
+  public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
+    m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH_LOW;
+    m_rampRate = (long) rampRate * 1000;
+    m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
+    //m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360;
+    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_currentVel = m_drive.m_rightFrontMotorVel;
+    m_currentPos = m_drive.m_rightFrontMotorPos;
+    m_targetPos = m_targetPos + m_currentPos;
+    m_targetVel = m_currentVel;
+    m_startTime = System.currentTimeMillis();
+    m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
+    m_counter = 0;
+  }
+
+  // Called every m_isRamptime the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentVel = m_drive.m_rightFrontMotorVel;
+    m_currentPos = m_drive.m_rightFrontMotorPos;
+    if (System.currentTimeMillis() - m_startTime < m_rampRate) {
+      // Ramping
+      m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
+      m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
+    } else if (m_targetPos - m_currentPos > m_rampDist) {
+      // Cruising
+      m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro);
+    } else {
+      // Deramp PID
+      m_drive.runDrivePositionPID(m_targetPos, m_targetGyro);
+    }
+    m_counter ++;
+  }
+
+  // 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((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) {
+      //return true;
+    }
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+
+public class DriveStraightAtVelocityPID extends CommandBase {
+  Drive m_drive;
+  double m_targetVel;
+  double m_targetGyro;
+  /**
+   * Creates a new DriveStraightAtVelocityPID.
+   * @param subsystem The drive subsystem
+   * @param targetVel The target velocity for the motors in units
+   */
+  public DriveStraightAtVelocityPID(Drive subsystem, double targetVel) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_targetVel = targetVel;
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
+    m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
+  }
+
+  // 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() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
+
+public class DriveStraightToPositionMM extends CommandBase {
+  Drive m_drive;
+  Pneumatics m_pneumatics;
+  double m_targetPosIn;
+  double m_targetPosOut;
+  double m_targetGyro;
+  boolean isGoneFast;
+  int i;
+  
+  /**
+   * Creates a new DriveToDistancePID.
+   * @param subsystem drive subsystem
+   * @param targetPos distance to travel in inches
+   */
+  public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_pneumatics = subsystem2;
+    try {
+      if (m_pneumatics.m_isSpeedShiftHigh) {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
+      } else {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
+      }
+    } catch (Exception e) {
+      System.err.println("Error In Motion Magic Switch Gains.");
+    }
+    addRequirements(m_drive);
+    //SmartDashboard.putNumber("Distance Target Inches", targetPos);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    System.err.println("PID START \n | \n |");
+    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
+    isGoneFast = false;
+    i = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    //System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
+    //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
+    //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
+    m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro);
+    SmartDashboard.putBoolean("MM Run", true);
+    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((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
+      SmartDashboard.putBoolean("MM Run", false);
+      return true;
+    } else {
+      if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
+        isGoneFast = true;
+      }
+      return false;
+    }
+  }
+}

src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
+
+public class DriveStraightToPositionPID extends CommandBase {
+  Drive m_drive;
+  Pneumatics m_pneumatics;
+  double m_targetPosIn;
+  double m_targetPosOut;
+  double m_targetGyro;
+  int i;
+  
+  /**
+   * Creates a new DriveToDistancePID.
+   * @param subsystem drive subsystem
+   * @param targetPos distance to travel in inches
+   */
+  public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_pneumatics = subsystem2;
+    try {
+      if (m_pneumatics.m_isSpeedShiftHigh) {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
+      } else {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
+      }
+    } catch (Exception e) {
+      System.err.println("Error In Motion Magic Switch Gains.");
+    }
+    addRequirements(m_drive);
+    //SmartDashboard.putNumber("Distance Target Inches", targetPos);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    //System.err.println("PID START \n | \n |");
+    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
+    i = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    //System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
+    //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
+    //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
+    m_drive.runDrivePositionPID(m_targetPosOut, m_targetGyro);
+  }
+
+  // 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((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){
+      return true;
+    } else {
+      i++;
+      //System.err.println(i);
+      return false;
+    }
+  }
+}

src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java

@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
+import frc4388.utility.controller.IHandController;
+
+public class DriveWithJoystick extends CommandBase {
+  private Drive m_drive;
+  private IHandController m_controller;
+  private Pneumatics m_pneumatics;
+
+  /**
+   * Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller.
+   * Applies a curved ramp to the input from the controllers to make the robot less "touchy".
+   * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
+   * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
+   * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   */
+  public DriveWithJoystick(Drive subsystem, Pneumatics subsystem2, IHandController controller) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_pneumatics = subsystem2;
+    m_controller = controller;
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double moveInput = m_controller.getLeftYAxis();
+    double steerInput = m_controller.getRightXAxis();
+    double moveOutput = 0;
+    double steerOutput = 0;
+    if (moveInput >= 0){
+      moveOutput = -Math.cos(1.571*moveInput)+1;
+    } else {
+      moveOutput = Math.cos(1.571*moveInput)-1;
+    }
+
+    double cosMultiplier;
+    double deadzone = .1;
+
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
+    } else {
+      cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
+    }
+
+    if (steerInput > 0){
+      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
+    } else if (steerInput < 0) {
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
+    } else {
+      steerOutput = 0;
+    }
+
+    /*
+    double outputLimit = 0.8;
+
+    boolean isMoveOutputLimited = false;
+    boolean isSteerOutputLimited = false;
+    
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      if (isMoveOutputLimited) {
+        if (moveOutput > outputLimit) {
+          moveOutput = outputLimit;
+        } else if(moveOutput < -outputLimit) {
+          moveOutput = -outputLimit;
+        }
+      }
+
+      if (isSteerOutputLimited) {
+        if (steerOutput > outputLimit) {
+          steerOutput = outputLimit;
+        } else if(steerOutput < -outputLimit) {
+          steerOutput = -outputLimit;
+        }
+      }
+    } 
+  */
+    
+    m_drive.driveWithInput(moveOutput, steerOutput);
+  }
+
+  // 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() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java

@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+import frc4388.utility.controller.IHandController;
+
+public class DriveWithJoystickAuxPID extends CommandBase {
+  Drive m_drive;
+  double m_targetGyro;
+  long lastTime;
+  IHandController m_controller;
+
+  /**
+   * Creates a new DriveWithJoystickAuxPID.
+   */
+  public DriveWithJoystickAuxPID(Drive subsystem, IHandController controller) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_controller = controller;
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    lastTime = System.currentTimeMillis();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    double moveInput = m_controller.getLeftYAxis();
+    double steerInput = m_controller.getRightXAxis();
+    double moveOutput = 0;
+    long deltaTime = System.currentTimeMillis() - lastTime;
+    lastTime = System.currentTimeMillis();
+    if (moveInput >= 0){
+      moveOutput = -Math.cos(1.571*moveInput)+1;
+    } else {
+      moveOutput = Math.cos(1.571*moveInput)-1;
+    }
+
+    m_targetGyro += 2 * steerInput * deltaTime;
+
+    m_targetGyro = MathUtil.clamp(m_targetGyro, 
+                                  currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
+                                  currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
+
+    m_drive.driveWithInputAux(moveOutput, m_targetGyro);
+
+    System.err.println("Target: " + m_targetGyro);
+    System.err.println("Current: " + currentGyro);
+    System.err.println();
+  }
+
+  // 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() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java

@@ -0,0 +1,142 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
+import frc4388.utility.controller.IHandController;
+
+public class DriveWithJoystickDriveStraight extends CommandBase {
+  Drive m_drive;
+  Pneumatics m_pneumatics;
+  double m_targetGyro, m_currentGyro;
+  double m_stopPos;
+  long m_currTime, m_deltaTime;
+  long m_deadTimeSteer, m_deadTimeMove;
+  long m_deadTimeout = 100;
+  IHandController m_controller;
+  boolean m_isInterrupted;
+  double highGearMultiplier = 1;
+  double lowGearMultiplier = 1;
+
+  /**
+   * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
+   * Applies a curved ramp to the input from the controllers to make the robot less "touchy".
+   * Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
+   * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
+   * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
+   * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   */
+  public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_pneumatics = m_drive.m_pneumaticsSubsystem;
+    m_controller = controller;
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_currTime = System.currentTimeMillis();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1);
+    double moveInput = -m_controller.getLeftYAxis();
+    double steerInput = m_controller.getRightXAxis();
+    double moveOutput = 0;
+    m_deltaTime = System.currentTimeMillis() - m_currTime;
+    m_currTime = System.currentTimeMillis();
+
+    if (m_isInterrupted) {
+      resetGyroTarget();
+      m_isInterrupted = false;
+    }
+
+    /* If steer stick is being used */
+    if (steerInput != 0) {
+      m_deadTimeSteer = m_currTime;
+    }
+
+    /* Curves the moveInput to be slightly more gradual at first */
+    if (moveInput >= 0) {
+      moveOutput = -Math.cos(1.571*moveInput)+1;
+    } else {
+      moveOutput = Math.cos(1.571*moveInput)-1;
+    }
+
+    /* 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 {
+      runDriveStraight(moveOutput);
+    }
+  }
+
+  private void runDriveWithInput(double move, double steer) {
+
+    double cosMultiplier;    
+    double steerOutput = 0;
+    double deadzone = .1;
+
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
+    } else {
+      cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
+    }
+
+    /* Curves the steer output to be similarily gradual */
+    if (steer > 0) {
+      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier;
+    } else if (steer < 0) {
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier;
+    }
+    m_drive.driveWithInput(move, steerOutput);
+    System.out.println("Driving With Input");
+  }
+
+  private void runDriveStraight(double move) {
+    m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
+    System.out.println("Driving Straight with Target: " + m_targetGyro);
+  }
+
+  /**
+   * set target angle to current angle (prevents buildup of gyro error).
+   */
+  private void resetGyroTarget() {
+    //m_targetGyro = m_currentGyro;
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      m_targetGyro =  m_currentGyro
+                    + highGearMultiplier * m_drive.getTurnRate();
+    } else {
+      m_targetGyro =  m_currentGyro
+                    + lowGearMultiplier * m_drive.getTurnRate();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_isInterrupted = interrupted;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,194 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
+import frc4388.utility.controller.IHandController;
+
+public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
+  Drive m_drive;
+  Pneumatics m_pneumatics;
+  double m_targetGyro, m_currentGyro;
+  double m_stopPos;
+  long m_currTime, m_deltaTime;
+  long m_deadTimeSteer, m_deadTimeMove;
+  long m_deadTimeout = 100;
+  IHandController m_controller;
+  boolean m_isInterrupted;
+
+  /* Deadassist Constants */
+  final float stopPosVelCoefLow = 1;
+  final float stopPosVelCoefHigh = 3;
+  final float cosMultiplierLow = 0.55f;
+  final float cosMultiplierHigh = 0.35f;
+  final float targetAngleCoefLow = 5;
+  final float targetAngleCoefHigh = 5;
+  final float gyroVelCoefLow = 1;
+  final float gyroVelCoefHigh = 3;
+
+  /* Deadassist Coeficients */
+  final float stopPosVelCoef = 1;
+  final float cosMultiplier = 0.55f;
+  final float targetAngleCoef = 5;
+  final float gyroVelCoef = 1;
+
+  /**
+   * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
+   * Applies a curved ramp to the input from the controllers to make the robot less "touchy".
+   * Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
+   * @param subsystemDrive pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
+   * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
+   * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   */
+  public DriveWithJoystickUsingDeadAssistPID(Drive subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystemDrive;
+    m_pneumatics = subsystemPneumatics;
+    m_controller = controller;
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_currTime = System.currentTimeMillis();
+    resetGyroTarget();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    double moveInput = -m_controller.getLeftYAxis();
+    double steerInput = m_controller.getRightXAxis();
+    double moveOutput = 0;
+    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;
+      m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
+                  + (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());
+    }
+    /* If steer stick is being used */
+    if (steerInput != 0) {
+      m_deadTimeSteer = m_currTime;
+    }
+
+    /* Curves the moveInput to be slightly more gradual at first */
+    if (moveInput >= 0) {
+      moveOutput = -Math.cos(1.571*moveInput)+1;
+    } else {
+      moveOutput = Math.cos(1.571*moveInput)-1;
+    }
+
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      runDriveWithInput(moveOutput, steerInput);
+      resetGyroTarget();
+    }
+    /* If move stick has been pressed within 1 sec */
+    else if (m_currTime - m_deadTimeMove < m_deadTimeout) {
+      /* 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 {
+        runDriveStraight(moveOutput);
+      }
+    }
+    /* If the move stick has not been used for 1 sec */
+    else {
+      runStoppedTurn(steerInput);
+    }
+  }
+
+  private void runDriveWithInput(double move, double steerInput) {
+    double cosMultiplier = .70;
+    double steerOutput = 0;
+    double deadzone = .1;
+    /* Curves the steer output to be similarily gradual */
+    if (steerInput > 0){
+      steerOutput = -(cosMultiplier - deadzone)*Math.cos(1.571*steerInput)+(cosMultiplier);
+    } else if (steerInput < 0) {
+      steerOutput = (cosMultiplier - deadzone)*Math.cos(1.571*steerInput)-(cosMultiplier);
+    }
+
+    m_drive.driveWithInput(move, steerOutput);
+    System.out.println("Driving With Input");
+  }
+
+  private void runDriveStraight(double move) {
+    m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
+    System.out.println("Driving Straight with Target: " + m_targetGyro);
+  }
+
+  private void runStoppedTurn(double steer) {
+    double cosMultiplier = 0.55;
+    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) > 200) {
+      m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
+    } else {
+      m_drive.driveWithInputAux(0, m_targetGyro);
+    }
+    System.out.println("Turning with Target: " + m_targetGyro);
+  }
+
+  /**
+   * If AuxPID is enabled, then update using the steer input
+   */
+  private void updateGyroTarget(double steerInput) {
+    m_targetGyro -= 5 * steerInput * m_deltaTime;
+    m_targetGyro = MathUtil.clamp(  m_targetGyro,
+                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/3),
+                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/3));
+  }
+
+  /**
+   * set target angle to current angle (prevents buildup of gyro error).
+   */
+  private void resetGyroTarget() {
+    //m_targetGyro = m_currentGyro;
+    m_targetGyro =  m_currentGyro
+                    + m_drive.getTurnRate();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_isInterrupted = interrupted;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java

@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.commands.auto.Wait;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class GotoCoordinates extends SequentialCommandGroup {
+  Drive m_drive;
+  Pneumatics m_pneumatics;
+
+  double m_xTarget;
+  double m_yTarget;
+  double m_currentAngle;
+  double m_hypotDist;
+  double m_endAngle;
+
+  /**
+   * Creates a new GotoPosition.
+   */
+  public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+    m_pneumatics = subsystem2;
+
+    m_xTarget = xTarget;
+    m_yTarget = yTarget;
+
+    m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
+
+    m_currentAngle = calcAngle();
+
+    m_endAngle = endAngle;
+
+    addCommands(  new TurnDegrees(m_drive, m_currentAngle),
+                  new Wait(m_drive, 0, 0),
+                  new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
+                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
+  }
+
+  public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
+    m_drive = subsystem;
+    m_pneumatics = subsystem2;
+
+    m_xTarget = xTarget;
+    m_yTarget = yTarget;
+
+    m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
+
+    m_currentAngle = calcAngle();
+
+    m_endAngle = m_currentAngle;
+
+    addCommands(  new TurnDegrees(m_drive, m_currentAngle),
+                  new Wait(m_drive, 0, 0),
+                  new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
+                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
+  }
+
+  public boolean isQuadrantThree() {
+    if ((m_xTarget < 0) && (m_yTarget < 0)) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  public double calcAngle() {
+    if (isQuadrantThree()) {
+      return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
+    } else {
+      return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
+    }
+  }
+
+}

src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* 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.drive;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Drive;
+
+public class PlaySongDrive extends CommandBase {
+  private Drive m_drive;
+  
+  /**
+   * Creates a new PlaySongDrive.
+   */
+  public PlaySongDrive(Drive subsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_drive.m_rightFrontMotor.set(0);
+    m_drive.m_leftFrontMotor.set(0);
+    m_drive.m_rightBackMotor.set(0);
+    m_drive.m_leftBackMotor.set(0);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_drive.playSong();
+    //System.err.println("Playing " + m_drive.m_orchestra.isPlaying());
+    //m_drive.m_driveTrain.feedWatchdog();
+  }
+
+  // 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() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/drive/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.drive;
+
+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(Drive subsystem, double targetAngle) {
+    // 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) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) {
+      return true;
+    }
+    return false;
+  }
+}
+