working slalom 9s run, working drivestraightatvelocity (needs some work)

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

@@ -36,7 +36,7 @@ public final class Constants {
         public static final boolean isRightMotorInverted = true;
         public static final boolean isLeftMotorInverted = false;
         public static final boolean isRightArcadeInverted = false;
-        public static final boolean isAuxPIDInverted = true;
+        public static final boolean isAuxPIDInverted = false;
 
         /* Drive Configuration */
         public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config

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

@@ -195,14 +195,14 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 1000));
+            .whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 5000));
 
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new TurnDegrees(m_robotDrive, 45));
+            .whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new Wait(m_robotDrive, 0, 0));
+            .whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
@@ -403,14 +403,14 @@ public class RobotContainer {
             //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-            //return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-            return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
+            return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
 
         } catch (Exception e) {
             System.err.println("ERROR");
         }
 
-        return new InstantCommand();    
+        return new InstantCommand();
     }
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(

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

@@ -29,7 +29,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    m_targetGyro = (m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
   }
 
   // Called every time the scheduler runs while the command is scheduled.

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

@@ -46,7 +46,7 @@ public class GotoCoordinatesRobotRelative extends SequentialCommandGroup {
     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));
+                  new TurnDegrees(m_drive, (m_endAngle - m_currentAngle) % 360));
   }
 
   public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {

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

@@ -412,7 +412,7 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
 
-    m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
+    m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); 
     m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
@@ -437,7 +437,7 @@ public class Drive extends SubsystemBase {
     m_rightBackMotor.follow(m_rightFrontMotor);
 
     m_driveTrain.feedWatchdog();
-
+  
   }
 
   /**