Replaced GotoCoordinate with Distance PID instead Motion Magic

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

@@ -54,7 +54,7 @@ public final class Constants {
         public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
         public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
         public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55);
-        public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
         public static final int DRIVE_CRUISE_VELOCITY = 25000;
         public static final int DRIVE_ACCELERATION = 21000;
 

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

@@ -192,7 +192,7 @@ public class RobotContainer {
        
         //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
         //This assumes that we are positioned against the right wall with our shooter facing the target.
-        return new SequentialCommandGroup(new Wait(0, m_robotDrive), 
+        return new SequentialCommandGroup(new Wait(m_robotDrive, 0), 
                                     //add aim command
                                     //add shooter command
                                     //new DriveStraightToPositionMM(m_robotDrive, 48.0),
@@ -208,7 +208,9 @@ public class RobotContainer {
 //Below this would be the picking up additional balls outside of those in the trench directly behind us
 
                                     //new GotoCoordinates(m_robotDrive, 36, 36),
-                                    new GotoCoordinates(m_robotDrive, 36, 36, -90));//,
+                                    //new DriveStraightToPositionPID(m_robotDrive, 160)
+                                    //new DriveStraightToPositionMM(m_robotDrive, 160)
+                                    new GotoCoordinates(m_robotDrive, 87, 47, -90));//,
                                     //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
                                     //new TurnDegrees(m_robotDrive, 75),
                                     //new DriveStraightToPositionMM(m_robotDrive, 18.0),

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);