All low gear PIDs WORKY, test auto command WORKY

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

@@ -66,13 +66,13 @@ public class DrivePositionMPAux extends CommandBase {
     if (System.currentTimeMillis() - m_startTime < m_rampRate) {
       // Ramping
       m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
-      m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+      m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
     } else if (m_targetPos - m_currentPos > m_rampDist) {
       // Cruising
-      m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
+      m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro);
     } else {
       // Deramp PID
-      m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
+      m_drive.runDrivePositionPID(m_targetPos, m_targetGyro);
     }
     m_counter ++;
   }

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

@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
   @Override
   public void execute() {
     //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
-    m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+    m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -58,7 +58,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   // 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 > 5){
+    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){
       return true;
     } else {
       i++;

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

@@ -24,7 +24,7 @@ public class TurnDegrees extends CommandBase {
   /**
    * Creates a new TurnDeg.
    */
-  public TurnDegrees(double targetAngle, Drive subsystem) {
+  public TurnDegrees(Drive subsystem, double targetAngle) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_targetAngle = targetAngle;