Merge branch 'master' into covid-shooter-revert

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/Robot.java

@@ -26,6 +26,9 @@ public class Robot extends TimedRobot {
   Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
+  double m_initialTime;
+  double m_currentTime;
+  double m_deltaTime;
 
   /**
    * This function is run when the robot is first started up and should be
@@ -67,10 +70,12 @@ public class Robot extends TimedRobot {
     /* Builds Autos */
     m_robotContainer.buildAutos();
     SmartDashboard.putString("Is Auto Start?", "NAH");
+    m_robotContainer.m_robotLime.limeOff();
   }
 
   @Override
   public void disabledPeriodic() {
+    m_robotContainer.resetOdometry(new Pose2d());
   }
 
   /**
@@ -78,12 +83,12 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousInit() {
-    
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
-    m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
+    m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
     m_robotContainer.setDriveGearState(true);
-    m_robotContainer.resetOdometry(new Pose2d());
+
+    m_initialTime = System.currentTimeMillis();
 
     //m_robotContainer.resetGyroYawRobotContainer(0);
 
@@ -108,6 +113,9 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousPeriodic() {
+    m_currentTime = System.currentTimeMillis();
+    m_deltaTime = m_currentTime - m_initialTime;
+    SmartDashboard.putNumber("Auto Path Time", (m_deltaTime)/1000);
   }
 
   @Override

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

@@ -38,14 +38,22 @@ import frc4388.robot.commands.auto.DriveOffLineBackward;
 import frc4388.robot.commands.auto.DriveOffLineForward;
 import frc4388.robot.commands.auto.EightBallAutoMiddle;
 import frc4388.robot.commands.auto.FiveBallAutoMiddle;
+import frc4388.robot.commands.auto.SequentialTest;
 import frc4388.robot.commands.auto.SixBallAutoMiddle;
+import frc4388.robot.commands.auto.Slalom;
+import frc4388.robot.commands.auto.TankDriveVelocity;
 import frc4388.robot.commands.auto.TenBallAutoMiddle;
 import frc4388.robot.commands.InterruptSubystem;
 import frc4388.robot.commands.auto.AutoPath1FromCenter;
+import frc4388.robot.commands.auto.Barrel;
+import frc4388.robot.commands.auto.BarrelMany;
+import frc4388.robot.commands.auto.BarrelStart;
+import frc4388.robot.commands.auto.Bounce;
 import frc4388.robot.commands.auto.Wait;
 import frc4388.robot.commands.climber.DisengageRachet;
 import frc4388.robot.commands.climber.RunClimberWithTriggers;
 import frc4388.robot.commands.climber.RunLevelerWithJoystick;
+import frc4388.robot.commands.drive.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.drive.DriveStraightToPositionMM;
 import frc4388.robot.commands.drive.DriveWithJoystick;
 import frc4388.robot.commands.drive.PlaySongDrive;
@@ -106,7 +114,7 @@ public class RobotContainer {
     /* Cameras */
     private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
     private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
-    private final LimeLight m_robotLime = new LimeLight();
+    public final LimeLight m_robotLime = new LimeLight();
 
     /* Controllers */
     private static XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
@@ -119,6 +127,10 @@ public class RobotContainer {
 
     SixBallAutoMiddle m_sixBallAutoMiddle;
 
+    SixBallAutoMiddle m_sixBallAutoMiddle0;
+
+    SixBallAutoMiddle m_sixBallAutoMiddle1;
+
     EightBallAutoMiddle m_eightBallAutoMiddle;
 
     DriveOffLineForward m_driveOffLineForward;
@@ -129,8 +141,21 @@ public class RobotContainer {
 
     TenBallAutoMiddle m_tenBallAutoMiddle;
 
+    Slalom m_slalom;
+
+    Barrel m_barrel;
+
+    BarrelStart m_barrelStart;
+
+    BarrelMany m_barrelMany;
+
+    Bounce m_bounce;
+
+    SequentialTest m_sequentialTest;
+
     public static boolean m_isShooterManual = false;
 
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -171,9 +196,8 @@ public class RobotContainer {
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
         // runs the storage not
-        //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
         m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage));
-        //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
+        m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
 
     /**
@@ -190,12 +214,14 @@ public class RobotContainer {
             .whenReleased(new InterruptSubystem(m_robotShooter))
             .whenReleased(new InterruptSubystem(m_robotShooterHood));
 
+
         // B driver test button
-        /*new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new TurnDegrees(m_robotDrive, 90));*/
+        new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
+            .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)
@@ -326,6 +352,51 @@ public class RobotContainer {
 
         m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
 
+        String[] sixBallAutoMiddle0Paths = new String[]{
+            "SixBallMid0"
+        };
+
+        m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
+
+        String[] sixBallAutoMiddle1Paths = new String[]{
+            "SixBallMid1"
+        };
+
+        m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
+
+        String[] slalom = new String[]{
+            "Slalom"
+        };
+
+        m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
+
+        String[] barrel = new String[]{
+            "BarrelStart"
+        };
+
+        m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
+        
+        String[] barrelStart = new String[]{
+            "Barrel"
+        };
+
+        m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
+
+        String[] bounce = new String[]{
+            "Bounce1",
+            "Bounce2",
+            "Bounce3",
+            "Bounce4"
+        };
+
+        m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce));
+        
+        String[] barrelMany = new String[]{
+            "BarrelManyWaypoints"
+        };
+
+        m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
+        
         String[] eightBallAutoMiddlePaths = new String[]{
             "EightBallMidComplete"
         };
@@ -336,7 +407,7 @@ public class RobotContainer {
             "DriveOffLineForward"
         };
 
-        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths));
+        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
 
         String[] driveOffLineBackwardPaths = new String[]{
             "DriveOffLineBackward"
@@ -355,8 +426,14 @@ public class RobotContainer {
             "TenBallMidComplete"
         };
 
-        m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter,
-                m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths));
+        //m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths));
+    
+        String[] sequentialTestPaths = new String[]{
+            "Seq1",
+            "Seq2"
+        };
+
+        m_sequentialTest = new SequentialTest(this, buildPaths(sequentialTestPaths));
     }
 
     /**
@@ -375,12 +452,15 @@ public class RobotContainer {
             SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
 
             //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-            //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-            //return m_driveOffLinfeBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            //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_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_bounce.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
         } catch (Exception e) {
             System.err.println("ERROR");
         }
@@ -506,6 +586,7 @@ public class RobotContainer {
         return m_driverXbox;
     }
 
+
     /**
      * Used for analog inputs like triggers and axises.
      * @return The IHandController interface for the Operator Controller.

src/main/java/frc4388/robot/commands/auto/Barrel.java

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class Barrel extends SequentialCommandGroup {
+  /**
+   * Creates a new Barrel.
+   */
+  public Barrel(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+   addCommands(
+     paths[0]
+   );
+  }
+}

src/main/java/frc4388/robot/commands/auto/BarrelMany.java

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class BarrelMany extends SequentialCommandGroup {
+  /**
+   * Creates a new BarrelMany.
+   */
+  public BarrelMany(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    addCommands(
+      paths[0]
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/BarrelStart.java

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class BarrelStart extends SequentialCommandGroup {
+  /**
+   * Creates a new BarrelStart.
+   */
+  public BarrelStart(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0],
+      //new Wait(drive, 0.01, 1),
+      new TankDriveVelocity(drive, 5, 5, 1.2) //my life be like oooooo aaaaaa ooooo aaaa
+    );
+    
+  }
+}

src/main/java/frc4388/robot/commands/auto/Bounce.java

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class Bounce extends SequentialCommandGroup {
+  /**
+   * Creates a new Bounce.
+   */
+  public Bounce(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0],
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[1],
+      new InstantCommand(() -> drive.switchReversed(false)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[2],
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[3],
+      new InstantCommand(() -> drive.switchReversed(false)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d()))
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java

@@ -7,8 +7,11 @@
 
 package frc4388.robot.commands.auto;
 
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
 import frc4388.robot.subsystems.Drive;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -18,11 +21,13 @@ public class DriveOffLineForward extends SequentialCommandGroup {
   /**
    * Creates a new DriveOffLineForward.
    */
-  public DriveOffLineForward(Drive drive, RamseteCommand[] paths) {
+  public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
 
     addCommands(
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
       paths[0]
     );
   }

src/main/java/frc4388/robot/commands/auto/SequentialTest.java

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class SequentialTest extends SequentialCommandGroup {
+  /**
+   * Creates a new SequentialTest.
+   */
+  public SequentialTest(RobotContainer m_robotContainer, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0],
+      new InstantCommand(() -> m_robotContainer.resetOdometry(new Pose2d())),
+      paths[1]
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/Slalom.java

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class Slalom extends SequentialCommandGroup {
+  /**
+   * Creates a new Slalom.
+   */
+  public Slalom(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0]
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java

@@ -15,10 +15,10 @@ public class TankDriveVelocity extends CommandBase {
   double m_leftTargetVel;
   double m_rightTargetVel;
 
-  double m_targetTime;
-  double m_firstTimeSec;
-  double m_currentTimeSec;
-  double m_diffSec;
+  long m_targetTime;
+  long m_firstTime;
+  long m_currentTime;
+  long m_diffTime;
 
   /**
    * Creates a new TankDriveVelocity.
@@ -28,24 +28,24 @@ public class TankDriveVelocity extends CommandBase {
     m_drive = subsystem;
     m_leftTargetVel = leftTargetVel;
     m_rightTargetVel = rightTargetVel;
-    m_targetTime = targetTime;
+    m_targetTime = (long) (targetTime * 1000);
     addRequirements(subsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_firstTimeSec = (System.currentTimeMillis() / 1000);
-    m_diffSec = 0;
+    m_firstTime = System.currentTimeMillis();
+    m_diffTime = 0;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_currentTimeSec = (System.currentTimeMillis() / 1000);
-    m_diffSec = m_currentTimeSec - m_firstTimeSec;
+    m_currentTime = System.currentTimeMillis();
+    m_diffTime = m_currentTime - m_firstTime;
     
-    if (m_diffSec < m_targetTime) {
+    if (m_diffTime < m_targetTime) {
       m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel);
     }
 
@@ -59,7 +59,7 @@ public class TankDriveVelocity extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_diffSec >= m_targetTime) {
+    if (m_diffTime >= m_targetTime) {
       return true;
     }
     return false;

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/commands/drive/TurnDegrees.java

@@ -38,6 +38,7 @@ public class TurnDegrees extends CommandBase {
   public void initialize() {
     m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
     m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    SmartDashboard.putNumber("Current Yaw Ticks", m_currentYawInTicks);
     m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks;
 
     i = 0;
@@ -48,10 +49,10 @@ public class TurnDegrees extends CommandBase {
   public void execute() {
     m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
 
-    m_drive.runTurningPID(m_targetAngleTicksOut);
+    m_drive.runTurningPID(m_targetAngleTicksIn);
 
-    //SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
-    //SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
+    SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
+    SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
 
     i++;
   }

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

@@ -46,6 +46,7 @@ public class Drive extends SubsystemBase {
   public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
   public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
   public static GyroBase m_pigeonGyro;
+  public boolean m_isReversed;
 
   /* Drive objects to manage Drive Train */
   public DifferentialDrive m_driveTrain;
@@ -339,9 +340,26 @@ public class Drive extends SubsystemBase {
     m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
     m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
 
-    m_odometry.update(Rotation2d.fromDegrees( getHeading()),
+    updateOdometry(m_isReversed);
+  }
+
+  public void updateOdometry(boolean reversed){
+    if (reversed){
+      m_odometry.update(Rotation2d.fromDegrees( -getHeading()-180),
+                                              -inchesToMeters(getDistanceInches(m_rightFrontMotor)),
+                                              inchesToMeters(getDistanceInches(m_leftFrontMotor)));
+    }
+    else
+    {
+      m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               inchesToMeters(getDistanceInches(m_leftFrontMotor)),
                                               -inchesToMeters(getDistanceInches(m_rightFrontMotor)));
+    }
+  }
+
+  public void switchReversed(boolean reversed)
+  {
+    m_isReversed = reversed;
   }
 
   /**
@@ -401,7 +419,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);
@@ -426,7 +444,7 @@ public class Drive extends SubsystemBase {
     m_rightBackMotor.follow(m_rightFrontMotor);
 
     m_driveTrain.feedWatchdog();
-
+  
   }
 
   /**

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

@@ -20,7 +20,7 @@ public class LimeLight extends SubsystemBase {
 
   public void limeOff(){
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
   }
   
   public void limeOn(){