Almost finished 6 ball auto, just a little too wide

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

@@ -70,7 +70,7 @@ public final class Constants {
         public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000;
         public static final int DRIVE_ACCELERATION_HIGH = 7000;
 
-        public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.16, 0.0, 0.0, 0.058, 0, 1.0);
+        public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0);
 
         /* Trajectory Constants */
         public static final double MAX_SPEED_METERS_PER_SECOND = 1.0;

src/main/java/frc4388/robot/Robot.java

@@ -80,7 +80,7 @@ public class Robot extends TimedRobot {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
-    m_robotContainer.setDriveGearState(false);
+    m_robotContainer.setDriveGearState(true);
     m_robotContainer.resetOdometry(new Pose2d());
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
 
@@ -93,7 +93,7 @@ public class Robot extends TimedRobot {
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
-      //m_autonomousCommand.schedule();
+      m_autonomousCommand.schedule();
       SmartDashboard.putString("Is Auto Start?", "YEA");
     }
   }
@@ -108,7 +108,7 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
-    m_robotContainer.setDriveGearState(false);
+    m_robotContainer.setDriveGearState(true);
 
     m_robotContainer.shiftClimberRachet(false);
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);

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

@@ -259,9 +259,13 @@ public class RobotContainer {
         //RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
 
         // Run path following command, then stop at the end.
-        return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+        try {
+            return m_sixBallAutoMiddle.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/auto/SixBallAutoMiddle.java

@@ -29,7 +29,6 @@ public class SixBallAutoMiddle extends SequentialCommandGroup {
     // super(new FooCommand(), new BarCommand());
 
     addCommands(
-      new Wait(drive, 0, 1),
       paths[0],
       paths[1]
     );

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

@@ -342,7 +342,7 @@ public class Drive extends SubsystemBase {
    * using the Differential Drive class to manage the two inputs
    */
   public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(move, steer);
+    m_driveTrain.arcadeDrive(-move, steer);
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
   }
@@ -573,7 +573,7 @@ public class Drive extends SubsystemBase {
       @Override
       public void reset() {
         // TODO Auto-generated method stub
-        resetGyroYaw();
+        resetGyroYaw(0);
       }
     
       @Override