From f7d8d41445df968b95f56a69a7f4568baf979e12 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 11:08:38 -0700
Subject: [PATCH 01/15] Replaced GotoCoordinate with Distance PID instead
 Motion Magic

---
 src/main/java/frc4388/robot/Constants.java                | 2 +-
 src/main/java/frc4388/robot/RobotContainer.java           | 6 ++++--
 src/main/java/frc4388/robot/commands/GotoCoordinates.java | 5 +++--
 src/main/java/frc4388/robot/commands/Wait.java            | 2 +-
 4 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index dc283e4..3f1d249 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/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;
 
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 2fda40a..78bfc4c 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/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),
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
index 5580cfc..d267cbe 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java
+++ b/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));
   }
 
diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java
index 934b508..ee7f63f 100644
--- a/src/main/java/frc4388/robot/commands/Wait.java
+++ b/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);

From a95e43d02d6bed70f0923d9309f8bf3d5e5032a5 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 11:37:35 -0700
Subject: [PATCH 02/15] Added Path to Intake 3 balls in 6 ball Autonomous

---
 .../java/frc4388/robot/RobotContainer.java    | 28 +++++++++++--------
 .../robot/commands/GotoCoordinates.java       |  2 +-
 .../java/frc4388/robot/commands/Wait.java     |  2 +-
 3 files changed, 19 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 78bfc4c..a00158e 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -117,7 +117,7 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 60));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
@@ -125,11 +125,11 @@ public class RobotContainer {
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0));
+            .whenPressed(new Wait(m_robotDrive, 0));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
+            .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 60));
      
         /* Driver Buttons */
         // sets solenoids into high gear
@@ -207,15 +207,21 @@ public class RobotContainer {
                                     //add shooter command
 //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 DriveStraightToPositionPID(m_robotDrive, 160)
-                                    //new DriveStraightToPositionMM(m_robotDrive, 160)
-                                    new GotoCoordinates(m_robotDrive, 87, 47, -90));//,
+                                    
+                                    new GotoCoordinates(m_robotDrive, 75, 44, -90),
+                                    //Start Intake Ball 1
+                                    new GotoCoordinates(m_robotDrive, 0, 12, 0),
+                                    new GotoCoordinates(m_robotDrive, 0, 28, 0),
+                                    //Start Intake Ball 2
+                                    new GotoCoordinates(m_robotDrive, 0, 8, 0),
+                                    new GotoCoordinates(m_robotDrive, 0, 28, 0),
+                                     //Start Intake Ball 3
+                                     new GotoCoordinates(m_robotDrive, 0, 8, 0)
+                                     /*Shoot 3 Balls*/ );
+
+
+                                    /*new GotoCoordinates(m_robotDrive, 0, 68.75, 0),*/
                                     //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                    //new TurnDegrees(m_robotDrive, 75),
-                                    //new DriveStraightToPositionMM(m_robotDrive, 18.0),
-                                    //new TurnDegrees(m_robotDrive, -45),
-                                    //new DriveStraightToPositionMM(m_robotDrive, 12.0));
     }
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
index d267cbe..9c3fbcf 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -44,7 +44,7 @@ public class GotoCoordinates extends SequentialCommandGroup {
 
 
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
-                  new Wait(m_drive, 1),
+                  new Wait(m_drive, 0.1),
                   new DriveStraightToPositionPID(m_drive, m_hypotDist), 
                   new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
   }
diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java
index ee7f63f..4288abb 100644
--- a/src/main/java/frc4388/robot/commands/Wait.java
+++ b/src/main/java/frc4388/robot/commands/Wait.java
@@ -21,7 +21,7 @@ public class Wait extends CommandBase {
   /**
    * Creates a new WaitCommand.
    */
-  public Wait(SubsystemBase subsystem, float seconds) {
+  public Wait(SubsystemBase subsystem, double seconds) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_waitTime = (long) (seconds * 1000);

From 47b9a1910a8fc714eba871b7423c360011f7cdf7 Mon Sep 17 00:00:00 2001
From: Kyra <101209@psdschools.org>
Date: Sat, 29 Feb 2020 12:04:18 -0700
Subject: [PATCH 03/15] Fixed some comments

---
 .../java/frc4388/robot/RobotContainer.java    | 19 ++-----------------
 1 file changed, 2 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index a00158e..3a164e1 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -191,23 +191,9 @@ public class RobotContainer {
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
        
         //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.
+        //This assumes that we are positioned against parallel to the initiation line with our back bumper lined up with the center target
         return new SequentialCommandGroup(new Wait(m_robotDrive, 0), 
-                                    //add aim command
-                                    //add shooter command
-                                    //new DriveStraightToPositionMM(m_robotDrive, 48.0),
-                                    /*new ParallelCommandGroup(
-                                        new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                            new DriveStraightToPositionMM(m_robotDrive, 36.0)),
-                                    new ParallelCommandGroup(
-                                         new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                            new DriveStraightToPositionMM(m_robotDrive, 36.0)),
-                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),*/
-                                    //add aim command 
-                                    //add shooter command
-//Below this would be the picking up additional balls outside of those in the trench directly behind us
-
-                                    
+                                    //shoot pre-loaded 3 balls
                                     new GotoCoordinates(m_robotDrive, 75, 44, -90),
                                     //Start Intake Ball 1
                                     new GotoCoordinates(m_robotDrive, 0, 12, 0),
@@ -219,7 +205,6 @@ public class RobotContainer {
                                      new GotoCoordinates(m_robotDrive, 0, 8, 0)
                                      /*Shoot 3 Balls*/ );
 
-
                                     /*new GotoCoordinates(m_robotDrive, 0, 68.75, 0),*/
                                     //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
     }

From 361145b7f8a31de5e6ea85fd1fe297097b8cf563 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 12:11:10 -0700
Subject: [PATCH 04/15] Motion Magic Tunin

Got it to drive straight at lower speeds.
---
 src/main/java/frc4388/robot/Constants.java      | 2 +-
 src/main/java/frc4388/robot/RobotContainer.java | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 3f1d249..fd1e83d 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/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.1, 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.2, 0.05, 0, 1.0);
         public static final int DRIVE_CRUISE_VELOCITY = 25000;
         public static final int DRIVE_ACCELERATION = 21000;
 
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index a00158e..9737315 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -117,7 +117,7 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 60));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 240));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)

From 7e479e91a5b468c3c02ad52960d3f9ee09a3a72a Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 13:02:54 -0700
Subject: [PATCH 05/15] Added 80% output limit to steer because turns too fast

---
 .../robot/commands/DriveWithJoystick.java     | 36 +++++++++++--------
 1 file changed, 22 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 54bc129..efddb8f 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -10,11 +10,13 @@ package frc4388.robot.commands;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 import frc4388.utility.controller.IHandController;
 
 public class DriveWithJoystick extends CommandBase {
   private Drive m_drive;
   private IHandController m_controller;
+  private Pneumatics m_pneumatics;
 
   /**
    * Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller.
@@ -58,23 +60,29 @@ public class DriveWithJoystick extends CommandBase {
     } else {
       steerOutput = 0;
     }
-    double tempOutputLimit = 0.8;
+    
+    double outputLimit = 0.8;
 
-    boolean isOutputLimited = false;
-
-    if (isOutputLimited) {
-      if (moveOutput > tempOutputLimit) {
-        moveOutput = tempOutputLimit;
-      } else if(moveOutput < -tempOutputLimit) {
-        moveOutput = -tempOutputLimit;
-      }
-
-      if (steerOutput > tempOutputLimit) {
-        steerOutput = tempOutputLimit;
-      } else if(steerOutput < -tempOutputLimit) {
-        steerOutput = -tempOutputLimit;
+    boolean isMoveOutputLimited = false;
+    boolean isSteerOutputLimited = true;
+    
+  if (m_pneumatics.m_isSpeedShiftHigh) {
+    if (isMoveOutputLimited) {
+      if (moveOutput > outputLimit) {
+        moveOutput = outputLimit;
+      } else if(moveOutput < -outputLimit) {
+        moveOutput = -outputLimit;
       }
     }
+
+    if (isSteerOutputLimited) {
+      if (steerOutput > outputLimit) {
+        steerOutput = outputLimit;
+      } else if(steerOutput < -outputLimit) {
+        steerOutput = -outputLimit;
+      }
+    }
+  }
     
     m_drive.driveWithInput(moveOutput, steerOutput);
   }

From 7859fa33185acc9448f99f1e1bb701782c7b2257 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 13:24:22 -0700
Subject: [PATCH 06/15] added 80% output only on high gear the right way

---
 .../robot/commands/DriveWithJoystick.java     | 41 +++++++++++--------
 1 file changed, 25 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index efddb8f..4d9777a 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -53,6 +53,13 @@ public class DriveWithJoystick extends CommandBase {
 
     double cosMultiplier = 1.0;
     double deadzone = .1;
+
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      cosMultiplier = 0.8;
+    } else {
+      cosMultiplier = 1.0;
+    }
+
     if (steerInput > 0){
       steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
     } else if (steerInput < 0) {
@@ -60,29 +67,31 @@ public class DriveWithJoystick extends CommandBase {
     } else {
       steerOutput = 0;
     }
-    
+
+    /*
     double outputLimit = 0.8;
 
     boolean isMoveOutputLimited = false;
-    boolean isSteerOutputLimited = true;
+    boolean isSteerOutputLimited = false;
     
-  if (m_pneumatics.m_isSpeedShiftHigh) {
-    if (isMoveOutputLimited) {
-      if (moveOutput > outputLimit) {
-        moveOutput = outputLimit;
-      } else if(moveOutput < -outputLimit) {
-        moveOutput = -outputLimit;
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      if (isMoveOutputLimited) {
+        if (moveOutput > outputLimit) {
+          moveOutput = outputLimit;
+        } else if(moveOutput < -outputLimit) {
+          moveOutput = -outputLimit;
+        }
       }
-    }
 
-    if (isSteerOutputLimited) {
-      if (steerOutput > outputLimit) {
-        steerOutput = outputLimit;
-      } else if(steerOutput < -outputLimit) {
-        steerOutput = -outputLimit;
+      if (isSteerOutputLimited) {
+        if (steerOutput > outputLimit) {
+          steerOutput = outputLimit;
+        } else if(steerOutput < -outputLimit) {
+          steerOutput = -outputLimit;
+        }
       }
-    }
-  }
+    } 
+  */
     
     m_drive.driveWithInput(moveOutput, steerOutput);
   }

From 6bdeaefc7cc0e358e0e2290dac11a3b56bef0abf Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 13:28:20 -0700
Subject: [PATCH 07/15] added pneumatics subsystem as param to
 driveWithJoysticks

---
 src/main/java/frc4388/robot/RobotContainer.java             | 2 +-
 src/main/java/frc4388/robot/commands/DriveWithJoystick.java | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index d422b47..e57c495 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -93,7 +93,7 @@ public class RobotContainer {
 
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
-        m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
+        m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController()));
         // drives intake with input from triggers on the opperator controller
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
         // runs the drum shooter in idle mode
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 4d9777a..3c6e88f 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -26,9 +26,10 @@ public class DriveWithJoystick extends CommandBase {
    * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
    * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
    */
-  public DriveWithJoystick(Drive subsystem, IHandController controller) {
+  public DriveWithJoystick(Drive subsystem, Pneumatics subsystem2, IHandController controller) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
+    m_pneumatics = subsystem2;
     m_controller = controller;
     addRequirements(m_drive);
   }

From a711fe43d4637b2cdbd1bab690ad0c56d5f35449 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 14:55:17 -0700
Subject: [PATCH 08/15] Made both auto commands groups, and added param to wait

---
 .../java/frc4388/robot/RobotContainer.java    | 24 +++--------
 .../robot/commands/AutoPath1FromCenter.java   | 42 +++++++++++++++++++
 .../robot/commands/AutoPath2FromRight.java    | 41 ++++++++++++++++++
 .../robot/commands/GotoCoordinates.java       |  2 +-
 .../java/frc4388/robot/commands/Wait.java     | 13 +++++-
 5 files changed, 102 insertions(+), 20 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
 create mode 100644 src/main/java/frc4388/robot/commands/AutoPath2FromRight.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index e57c495..9900e90 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -28,6 +28,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
+import frc4388.robot.commands.AutoPath1FromCenter;
+import frc4388.robot.commands.AutoPath2FromRight;
 import frc4388.robot.commands.DrivePositionMPAux;
 import frc4388.robot.commands.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.DriveStraightToPositionMM;
@@ -117,7 +119,7 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 240));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 24));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
@@ -125,7 +127,7 @@ public class RobotContainer {
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new Wait(m_robotDrive, 0));
+            .whenPressed(new Wait(m_robotDrive, 0, 0));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
@@ -190,23 +192,9 @@ public class RobotContainer {
         // Run path following command, then stop at the end.
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
        
-        //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 parallel to the initiation line with our back bumper lined up with the center target
-        return new SequentialCommandGroup(new Wait(m_robotDrive, 0), 
-                                    //shoot pre-loaded 3 balls
-                                    new GotoCoordinates(m_robotDrive, 75, 44, -90),
-                                    //Start Intake Ball 1
-                                    new GotoCoordinates(m_robotDrive, 0, 12, 0),
-                                    new GotoCoordinates(m_robotDrive, 0, 28, 0),
-                                    //Start Intake Ball 2
-                                    new GotoCoordinates(m_robotDrive, 0, 8, 0),
-                                    new GotoCoordinates(m_robotDrive, 0, 28, 0),
-                                     //Start Intake Ball 3
-                                     new GotoCoordinates(m_robotDrive, 0, 8, 0)
-                                     /*Shoot 3 Balls*/ );
+        //return new AutoPath1FromCenter(m_robotDrive);
+        return new AutoPath2FromRight(m_robotDrive);
 
-                                    /*new GotoCoordinates(m_robotDrive, 0, 68.75, 0),*/
-                                    //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
     }
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(
diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
new file mode 100644
index 0000000..ced8a88
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+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 AutoPath1FromCenter extends SequentialCommandGroup {
+  Drive m_drive;
+
+  /**
+   * Creates a new AutoPath1FromCenter.
+   */
+  public AutoPath1FromCenter(Drive subsystem) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+
+    addCommands(  new Wait(m_drive, 0, 1), 
+                  //shoot pre-loaded 3 balls
+                  new GotoCoordinates(m_drive, 75, 44, -90),
+                  //Start Intake Ball 1
+                  new GotoCoordinates(m_drive, 0, 12, 0),
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 2
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 3
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new Wait(m_drive, 0, 2)
+                  //Shoot 3 Balls
+                  );
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
new file mode 100644
index 0000000..e1ebaa0
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+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 AutoPath2FromRight extends SequentialCommandGroup {
+  Drive m_drive;
+
+  /**
+   * Creates a new AutoPath2FromRight.
+   */
+  public AutoPath2FromRight(Drive subsystem) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+    
+    addCommands(  new Wait(m_drive, 0, 1),
+                  new GotoCoordinates(m_drive, 0, 77, 0),
+                  //Start Intake Ball 1
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 2
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  //Shoot 5 Balls
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 1 (second round)
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new Wait(m_drive, 0, 2)
+                  );
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
index 9c3fbcf..cfa8d2e 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -44,7 +44,7 @@ public class GotoCoordinates extends SequentialCommandGroup {
 
 
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
-                  new Wait(m_drive, 0.1),
+                  new Wait(m_drive, 0, 0),
                   new DriveStraightToPositionPID(m_drive, m_hypotDist), 
                   new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
   }
diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java
index 4288abb..580d279 100644
--- a/src/main/java/frc4388/robot/commands/Wait.java
+++ b/src/main/java/frc4388/robot/commands/Wait.java
@@ -17,15 +17,19 @@ public class Wait extends CommandBase {
   long m_waitTime;
   long m_currentTime;
   SubsystemBase m_subsystem;
+  int m_waitNum;
+
+  int counter  = 0;
 
   /**
    * Creates a new WaitCommand.
    */
-  public Wait(SubsystemBase subsystem, double seconds) {
+  public Wait(SubsystemBase subsystem, double seconds, int waitNum) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_waitTime = (long) (seconds * 1000);
     m_subsystem = subsystem;
+    m_waitNum = waitNum;
 
     addRequirements(m_subsystem);
   }
@@ -40,8 +44,15 @@ public class Wait extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+
+    if (counter == 0) {
+      SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
+    }
+
     m_currentTime = System.currentTimeMillis();
     SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
+
+    counter ++;
   }
 
   // Called once the command ends or is interrupted.

From 48b303fdfee2d570ca1ec7d576d2cd3773f79b1e Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 15:38:59 -0700
Subject: [PATCH 09/15] Started Path 2, Made endAngle an optional param

---
 .../java/frc4388/robot/RobotContainer.java    |  1 -
 .../robot/commands/AutoPath1FromCenter.java   | 10 +++++-----
 .../robot/commands/AutoPath2FromRight.java    | 14 ++++++++------
 .../robot/commands/GotoCoordinates.java       | 19 +++++++++++++++++--
 4 files changed, 30 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 9900e90..bdae26e 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -191,7 +191,6 @@ public class RobotContainer {
         RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
         // Run path following command, then stop at the end.
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-       
         //return new AutoPath1FromCenter(m_robotDrive);
         return new AutoPath2FromRight(m_robotDrive);
 
diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
index ced8a88..e941c05 100644
--- a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
+++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
@@ -28,13 +28,13 @@ public class AutoPath1FromCenter extends SequentialCommandGroup {
                   //shoot pre-loaded 3 balls
                   new GotoCoordinates(m_drive, 75, 44, -90),
                   //Start Intake Ball 1
-                  new GotoCoordinates(m_drive, 0, 12, 0),
-                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  new GotoCoordinates(m_drive, 0, 12),
+                  new GotoCoordinates(m_drive, 0, 28),
                   //Start Intake Ball 2
-                  new GotoCoordinates(m_drive, 0, 8, 0),
-                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  new GotoCoordinates(m_drive, 0, 8),
+                  new GotoCoordinates(m_drive, 0, 28),
                   //Start Intake Ball 3
-                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new GotoCoordinates(m_drive, 0, 8),
                   new Wait(m_drive, 0, 2)
                   //Shoot 3 Balls
                   );
diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
index e1ebaa0..2411523 100644
--- a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
+++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
@@ -25,16 +25,18 @@ public class AutoPath2FromRight extends SequentialCommandGroup {
     m_drive = subsystem;
     
     addCommands(  new Wait(m_drive, 0, 1),
-                  new GotoCoordinates(m_drive, 0, 77, 0),
+                  new GotoCoordinates(m_drive, 0, 77),
                   //Start Intake Ball 1
-                  new GotoCoordinates(m_drive, 0, 8, 0),
-                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  new GotoCoordinates(m_drive, 0, 8),
+                  new GotoCoordinates(m_drive, 0, 28),
                   //Start Intake Ball 2
-                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new GotoCoordinates(m_drive, 0, 8),
                   //Shoot 5 Balls
-                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  new GotoCoordinates(m_drive, 0, 28),
                   //Start Intake Ball 1 (second round)
-                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new GotoCoordinates(m_drive, 0, 8),
+                  //Start Moving to 4th Ball
+                  new GotoCoordinates(m_drive, 60, -50),
                   new Wait(m_drive, 0, 2)
                   );
   }
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
index cfa8d2e..244905b 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -38,10 +38,25 @@ public class GotoCoordinates extends SequentialCommandGroup {
 
     m_currentAngle = calcAngle();
 
-    SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle); 
-
     m_endAngle = endAngle;
 
+    addCommands(  new TurnDegrees(m_drive, m_currentAngle),
+                  new Wait(m_drive, 0, 0),
+                  new DriveStraightToPositionPID(m_drive, m_hypotDist), 
+                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
+  }
+
+  public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) {
+    m_drive = subsystem;
+
+    m_xTarget = xTarget;
+    m_yTarget = yTarget;
+
+    m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
+
+    m_currentAngle = calcAngle();
+
+    m_endAngle = m_currentAngle;
 
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
                   new Wait(m_drive, 0, 0),

From d8f15ccdb0b26f150faf350ceb7e88073dc7e89f Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 16:01:39 -0700
Subject: [PATCH 10/15] Tuned Motion Magic Gains pretty well

---
 src/main/java/frc4388/robot/Constants.java      | 6 +++---
 src/main/java/frc4388/robot/RobotContainer.java | 6 +++---
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index fd1e83d..1f4cf3a 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -54,9 +54,9 @@ 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.1, 0.0, 0.2, 0.05, 0, 1.0);
-        public static final int DRIVE_CRUISE_VELOCITY = 25000;
-        public static final int DRIVE_ACCELERATION = 21000;
+        public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0);
+        public static final int DRIVE_CRUISE_VELOCITY = 30000;
+        public static final int DRIVE_ACCELERATION = 23000;
 
         public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
         public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index bdae26e..4bd17d4 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -119,7 +119,7 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 24));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 12));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
@@ -191,8 +191,8 @@ public class RobotContainer {
         RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
         // Run path following command, then stop at the end.
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-        //return new AutoPath1FromCenter(m_robotDrive);
-        return new AutoPath2FromRight(m_robotDrive);
+        return new AutoPath1FromCenter(m_robotDrive);
+        //return new AutoPath2FromRight(m_robotDrive);
 
     }
     TrajectoryConfig getTrajectoryConfig() {

From 26a208524ee8d6b6a0b6e10a46c9006724aa2fa8 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sun, 1 Mar 2020 12:33:09 -0700
Subject: [PATCH 11/15] Finished High Gear Velocity and Turning, Working on
 High Gear Motion Magic

---
 src/main/java/frc4388/robot/Constants.java         | 10 +++++-----
 src/main/java/frc4388/robot/RobotContainer.java    |  4 ++--
 .../robot/commands/DriveStraightToPositionMM.java  | 14 ++++++++++++--
 src/main/java/frc4388/robot/subsystems/Drive.java  | 10 +++++++++-
 4 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 1f4cf3a..1e67858 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -52,16 +52,16 @@ public final class Constants {
 
         /* PID Constants Drive*/
         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_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0);
+        public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55);
         public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0);
         public static final int DRIVE_CRUISE_VELOCITY = 30000;
         public static final int DRIVE_ACCELERATION = 23000;
 
         public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
-        public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
-        public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.55);
-        public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55);
+        public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
         public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000;
         public static final int DRIVE_ACCELERATION_HIGH = 7000;
 
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 4bd17d4..3c52d68 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -119,11 +119,11 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 12));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
+            .whenPressed(new TurnDegrees(m_robotDrive, 90));
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
index 925f07a..b7c74bb 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
@@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 
 public class DriveStraightToPositionMM extends CommandBase {
   Drive m_drive;
+  Pneumatics m_pneumatics;
   double m_targetPosIn;
   double m_targetPosOut;
   double m_targetGyro;
@@ -25,10 +27,18 @@ public class DriveStraightToPositionMM extends CommandBase {
    * @param subsystem drive subsystem
    * @param targetPos distance to travel in inches
    */
-  public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
+  public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
+    try {
+      if (m_pneumatics.m_isSpeedShiftHigh) {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
+      } else {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
+      }
+    } catch (Exception e) {
+      System.err.println("Error In Motion Magic Switch Gains.");
+    }
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 8c2e2bd..058c05a 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -126,7 +126,15 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* PID for Front Motor Control in Teleop */
-    setRightMotorGains(false);
+    try {
+      if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
+        setRightMotorGains(true);
+      } else {
+        setRightMotorGains(false);
+      }
+    } catch (Exception e) {
+      System.err.println("Error while trying to switch gains.");
+    }
 
     /* PID for Back Motor Control in Tank Drive Vel */
     m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);

From 92df6a3d5f3177e56a44186f00530d93421b46f0 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sun, 1 Mar 2020 13:34:54 -0700
Subject: [PATCH 12/15] Switched All PIDs to High Gear

Motion Magic and Position need testing.
---
 src/main/java/frc4388/robot/Constants.java         |  2 +-
 src/main/java/frc4388/robot/RobotContainer.java    |  2 +-
 .../robot/commands/DriveStraightToPositionPID.java | 14 ++++++++++++--
 3 files changed, 14 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 1e67858..e268eb3 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -58,7 +58,7 @@ public final class Constants {
         public static final int DRIVE_CRUISE_VELOCITY = 30000;
         public static final int DRIVE_ACCELERATION = 23000;
 
-        public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
+        public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5);
         public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
         public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55);
         public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 3c52d68..48a40c4 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -131,7 +131,7 @@ public class RobotContainer {
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 60));
+            .whenPressed(new DriveStraightToPositionPID(m_robotDrive, m_robotPneumatics, 60));
      
         /* Driver Buttons */
         // sets solenoids into high gear
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
index c31944e..c8a339b 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
@@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 
 public class DriveStraightToPositionPID extends CommandBase {
   Drive m_drive;
+  Pneumatics m_pneumatics;
   double m_targetPosIn;
   double m_targetPosOut;
   double m_targetGyro;
@@ -24,10 +26,18 @@ public class DriveStraightToPositionPID extends CommandBase {
    * @param subsystem drive subsystem
    * @param targetPos distance to travel in inches
    */
-  public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
+  public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
+    try {
+      if (m_pneumatics.m_isSpeedShiftHigh) {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
+      } else {
+        m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
+      }
+    } catch (Exception e) {
+      System.err.println("Error In Motion Magic Switch Gains.");
+    }
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }

From 21a7c5517a420fd11d7be99b89202a8cea252c76 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sun, 1 Mar 2020 13:48:18 -0700
Subject: [PATCH 13/15] Finished all PIDs in high gear and added pneumatics
 subsystem literally everywhere

---
 .../java/frc4388/robot/RobotContainer.java    |  4 ++--
 .../robot/commands/AutoPath1FromCenter.java   | 17 ++++++++++-------
 .../robot/commands/AutoPath2FromRight.java    | 19 +++++++++++--------
 .../commands/DriveStraightToPositionMM.java   |  1 +
 .../commands/DriveStraightToPositionPID.java  |  1 +
 .../robot/commands/GotoCoordinates.java       | 12 ++++++++----
 6 files changed, 33 insertions(+), 21 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 48a40c4..66537a3 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -191,8 +191,8 @@ public class RobotContainer {
         RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
         // Run path following command, then stop at the end.
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-        return new AutoPath1FromCenter(m_robotDrive);
-        //return new AutoPath2FromRight(m_robotDrive);
+        return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics);
+        //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics);
 
     }
     TrajectoryConfig getTrajectoryConfig() {
diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
index e941c05..4ec080a 100644
--- a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
+++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
@@ -9,32 +9,35 @@ package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 
 // 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 AutoPath1FromCenter extends SequentialCommandGroup {
   Drive m_drive;
+  Pneumatics m_pneumatics;
 
   /**
    * Creates a new AutoPath1FromCenter.
    */
-  public AutoPath1FromCenter(Drive subsystem) {
+  public AutoPath1FromCenter(Drive subsystem, Pneumatics subsystem2) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
     m_drive = subsystem;
+    m_pneumatics = subsystem2;
 
     addCommands(  new Wait(m_drive, 0, 1), 
                   //shoot pre-loaded 3 balls
-                  new GotoCoordinates(m_drive, 75, 44, -90),
+                  new GotoCoordinates(m_drive, m_pneumatics, 75, 44, -90),
                   //Start Intake Ball 1
-                  new GotoCoordinates(m_drive, 0, 12),
-                  new GotoCoordinates(m_drive, 0, 28),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 12),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
                   //Start Intake Ball 2
-                  new GotoCoordinates(m_drive, 0, 8),
-                  new GotoCoordinates(m_drive, 0, 28),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
                   //Start Intake Ball 3
-                  new GotoCoordinates(m_drive, 0, 8),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
                   new Wait(m_drive, 0, 2)
                   //Shoot 3 Balls
                   );
diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
index 2411523..43657c3 100644
--- a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
+++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
@@ -9,34 +9,37 @@ package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 
 // 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 AutoPath2FromRight extends SequentialCommandGroup {
   Drive m_drive;
+  Pneumatics m_pneumatics;
 
   /**
    * Creates a new AutoPath2FromRight.
    */
-  public AutoPath2FromRight(Drive subsystem) {
+  public AutoPath2FromRight(Drive subsystem, Pneumatics subsystem2) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
     m_drive = subsystem;
+    m_pneumatics = subsystem2;
     
     addCommands(  new Wait(m_drive, 0, 1),
-                  new GotoCoordinates(m_drive, 0, 77),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 77),
                   //Start Intake Ball 1
-                  new GotoCoordinates(m_drive, 0, 8),
-                  new GotoCoordinates(m_drive, 0, 28),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
                   //Start Intake Ball 2
-                  new GotoCoordinates(m_drive, 0, 8),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
                   //Shoot 5 Balls
-                  new GotoCoordinates(m_drive, 0, 28),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 28),
                   //Start Intake Ball 1 (second round)
-                  new GotoCoordinates(m_drive, 0, 8),
+                  new GotoCoordinates(m_drive, m_pneumatics, 0, 8),
                   //Start Moving to 4th Ball
-                  new GotoCoordinates(m_drive, 60, -50),
+                  new GotoCoordinates(m_drive, m_pneumatics, 60, -50),
                   new Wait(m_drive, 0, 2)
                   );
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
index b7c74bb..c9aa927 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
@@ -30,6 +30,7 @@ public class DriveStraightToPositionMM extends CommandBase {
   public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
+    m_pneumatics = subsystem2;
     try {
       if (m_pneumatics.m_isSpeedShiftHigh) {
         m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
index c8a339b..a457e26 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
@@ -29,6 +29,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
+    m_pneumatics = subsystem2;
     try {
       if (m_pneumatics.m_isSpeedShiftHigh) {
         m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2;
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
index 244905b..6ca3deb 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -10,12 +10,14 @@ package frc4388.robot.commands;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 
 // 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 GotoCoordinates extends SequentialCommandGroup {
   Drive m_drive;
+  Pneumatics m_pneumatics;
 
   double m_xTarget;
   double m_yTarget;
@@ -26,10 +28,11 @@ public class GotoCoordinates extends SequentialCommandGroup {
   /**
    * Creates a new GotoPosition.
    */
-  public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) {
+  public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
     m_drive = subsystem;
+    m_pneumatics = subsystem2;
 
     m_xTarget = xTarget;
     m_yTarget = yTarget;
@@ -42,12 +45,13 @@ public class GotoCoordinates extends SequentialCommandGroup {
 
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
                   new Wait(m_drive, 0, 0),
-                  new DriveStraightToPositionPID(m_drive, m_hypotDist), 
+                  new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
                   new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
   }
 
-  public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) {
+  public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
     m_drive = subsystem;
+    m_pneumatics = subsystem2;
 
     m_xTarget = xTarget;
     m_yTarget = yTarget;
@@ -60,7 +64,7 @@ public class GotoCoordinates extends SequentialCommandGroup {
 
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
                   new Wait(m_drive, 0, 0),
-                  new DriveStraightToPositionPID(m_drive, m_hypotDist), 
+                  new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
                   new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
   }
 

From ec040e3432c80a7170b351a81b74bee6c5526a60 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sun, 1 Mar 2020 14:01:35 -0700
Subject: [PATCH 14/15] Fixed PIDs Initialization, always starts in Low Gear

---
 src/main/java/frc4388/robot/Robot.java          | 4 ++--
 src/main/java/frc4388/robot/RobotContainer.java | 4 ++++
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 4f894c9..9e827dd 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -77,7 +77,7 @@ public class Robot extends TimedRobot {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
-    //m_robotContainer.setDriveGearState(true);
+    m_robotContainer.setDriveGearState(false);
     m_robotContainer.resetOdometry();
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
 
@@ -105,7 +105,7 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
-    //m_robotContainer.setDriveGearState(true);
+    m_robotContainer.setDriveGearState(false);
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
     
     // This makes sure that the autonomous stops running when
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 66537a3..5ce2fdc 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -35,6 +35,7 @@ import frc4388.robot.commands.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.DriveStraightToPositionMM;
 import frc4388.robot.commands.DriveStraightToPositionPID;
 import frc4388.robot.commands.DriveWithJoystick;
+import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
 import frc4388.robot.commands.GotoCoordinates;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
@@ -95,7 +96,10 @@ public class RobotContainer {
 
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
+        
         m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController()));
+        //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController()));
+        
         // drives intake with input from triggers on the opperator controller
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
         // runs the drum shooter in idle mode

From 8313ff6b7c69f3dba94e565b26b75896eb581243 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sun, 1 Mar 2020 14:57:05 -0700
Subject: [PATCH 15/15] Update RobotContainer.java

---
 src/main/java/frc4388/robot/RobotContainer.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index ba33991..4793fc2 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -28,7 +28,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
-import frc4388.robot.Constants.*
+import frc4388.robot.Constants.*;
 import frc4388.robot.commands.AutoPath1FromCenter;
 import frc4388.robot.commands.AutoPath2FromRight;
 import frc4388.robot.commands.CalibrateShooter;
@@ -349,4 +349,4 @@ public class RobotContainer {
     {
         return m_driverXbox.getJoyStick();
     }
-}
\ No newline at end of file
+}