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

PathWeaver/Paths/Barrel

@@ -9,4 +9,4 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name
 5.802254718804354,-1.95509476261032,0.6432550409070421,-1.2184734909489143,true,
 7.020728209753269,-3.3096414352895698,1.6019524576434971,-0.8164390903820133,true,
 8.239201700702182,-3.0746059395735355,0.2102949172196098,0.5009967145525991,true,
-7.608316949043354,-2.286,-2.245,0.73,true,
+7.608316949043354,-2.286,-1.7503959286220452,0.73,true,

PathWeaver/Paths/Slalom

@@ -1,14 +1,13 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Name
-1.1633962507247309,-3.693120401984152,2.30034323262256,0.058245899632803244,true,
+1.2,-3.8989,1.9117168073181727,0.007854970044450571,true,
-2.85812587772982,-2.412795464794176,0.8102539457579074,0.6494401855311474,true,
+2.6292755266378918,-2.5426835019004055,1.0947705984667904,0.6679956194034657,true,
-4.571410938607228,-2.004575919603169,2.022542292082716,0.05566630161695518,true,
+4.571410938607228,-2.1715748244540354,2.022542292082716,0.05566630161695518,true,
-6.105326805385555,-2.3509440185531143,0.8734927301599473,-0.5329783419602319,false,
+6.173363396250723,-2.412795464794176,0.8224254383303222,-0.4893581628618532,false,
-7.150616246859498,-3.6683798234877276,1.1442517554596412,-0.7174767763963148,true,
+7.101135089866648,-3.526121497133286,0.8411796688784383,-0.7978836565096956,true,
-8.202090832957547,-3.618898666494878,0.6741807640275734,0.6370698962829353,true,
+8.233016556078077,-3.557047220253817,0.44533041293564324,0.4515155575597496,true,
-8.43712632867358,-2.456091477162919,-0.8721053919989696,1.107140887715003,true,
+8.344349159311989,-2.6663863943825286,-0.5999590285382981,0.9834379952328802,true,
-7.076394511370224,-2.4808320556593437,-0.5195521484249168,-1.2122883463248084,true,
+7.169171680731816,-2.4993874895316623,-0.5257372930490236,-0.7484024995168457,true,
-6.204289119371255,-3.581787798750241,-0.9215865489918196,-0.6185144624106163,true,
+6.519731495200669,-3.785897571345744,-0.7916985118855884,-0.38966411131868783,true,
-4.7384098434580935,-3.903415319203762,-1.0282413438142206,-0.047219421650161054,false,
+4.868297880564323,-4.00237763318946,-1.286510081814082,0.012370289248212263,true,
-3.1179019519422786,-3.674564968111833,-1.3854723957997805,0.5319224376731304,true,
+2.8952367454744574,-3.0993465180699604,-0.3587383881981583,2.2019114861817948,true,
-2.1282788120852927,-2.7839041422405457,-1.2679546479417638,1.6019524576434963,true,
+0.7366212716614056,-2.313833150808477,-0.8040688011338014,0.22266520646782118,true,
-0.5572520775623268,-2.140649101333505,-0.7236619210204216,0.04948115699284905,true,

PathWeaver/pathweaver.json

@@ -1,9 +1,9 @@
 {
   "lengthUnit": "Meter",
   "exportUnit": "Always Meters",
-  "maxVelocity": 2.3,
+  "maxVelocity": 2.6,
   "maxAcceleration": 2.7,
   "wheelBase": 0.648,
-  "gameName": "Barrel Racing Path",
+  "gameName": "Slalom Path",
   "outputDir": ".."
 }

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_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-            return m_barrelStart.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();
-
+  
   }
 
   /**