Replaced GotoCoordinate with Distance PID instead Motion Magic

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

@@ -43,8 +43,9 @@ public class GotoCoordinates extends SequentialCommandGroup {
     m_endAngle = endAngle;
 
 
-    addCommands(  new TurnDegrees(m_drive, m_currentAngle), 
-                  new DriveStraightToPositionMM(m_drive, m_hypotDist), 
+    addCommands(  new TurnDegrees(m_drive, m_currentAngle),
+                  new Wait(m_drive, 1),
+                  new DriveStraightToPositionPID(m_drive, m_hypotDist), 
                   new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
   }
 

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

@@ -21,7 +21,7 @@ public class Wait extends CommandBase {
   /**
    * Creates a new WaitCommand.
    */
-  public Wait(float seconds, SubsystemBase subsystem) {
+  public Wait(SubsystemBase subsystem, float seconds) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_waitTime = (long) (seconds * 1000);