From 247a513e9ae1afc378466bf3360958f73ca29f02 Mon Sep 17 00:00:00 2001
From: Rishabh <57200@psdschools.org>
Date: Tue, 11 Feb 2020 17:35:16 -0700
Subject: [PATCH 01/31] Start shooter stuff

---
 build.gradle                                        |  2 +-
 src/main/java/frc4388/robot/subsystems/Drive.java   |  2 +-
 src/main/java/frc4388/robot/subsystems/Storage.java | 12 ++++++++++++
 3 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/build.gradle b/build.gradle
index e00a044..e0194f3 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2020.1.2"
+    id "edu.wpi.first.GradleRIO" version "2020.2.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 997f16e..f23bad4 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -101,7 +101,7 @@ public class Drive extends ProfiledPIDSubsystem {
     m_driveTrain.arcadeDrive(move, steer);
   }
 
-  public double getGyroYaw() {
+public double getGyroYaw() {
     double[] ypr = new double[3];
     m_pigeon.getYawPitchRoll(ypr);
     return ypr[0];
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 7b85a0d..d3defc1 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -45,4 +45,16 @@ public class Storage extends SubsystemBase {
     m_storageMotor.set(input);
     boolean beam_on = m_beamSensors[0].get();
   }
+
+  /**
+   * Prepares storage for shooting
+   */
+  public void storageAim() {
+
+  }
+    /*
+    *If shooting move storage motor until top sensor is tripped
+    *If intaking move storage motor until bottom sensor is tripped
+    *
+    */
 }

From ee9bd4d3017fdcf5857798d3292dbdc1d5ba6873 Mon Sep 17 00:00:00 2001
From: Rishabh <57200@psdschools.org>
Date: Tue, 11 Feb 2020 20:07:35 -0700
Subject: [PATCH 02/31] Shooter gains fix

---
 src/main/java/frc4388/robot/Constants.java    |  9 ++-------
 .../java/frc4388/robot/RobotContainer.java    |  2 +-
 .../frc4388/robot/subsystems/Storage.java     | 20 ++++++++++---------
 3 files changed, 14 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index bcd15f8..318ac48 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -108,13 +108,8 @@ public final class Constants {
         public static final int PID_PRIMARY = 0;
 
         /* PID Gains */
-        public static final double storP = 0.1;
-        public static final double storI = 1e-4;
-        public static final double storD = 1.0;
-        public static final double storIz = 0.0;
-        public static final double storF = 0.0;
-        public static final double storkmaxOutput = 1.0;
-        public static final double storkminOutput = -1.0;
+        public static final double STORAGE_MIN_OUTPUT = -1.0;
+        public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
     }
   
     public static final class LEDConstants {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 2585fbc..bf4a506 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -137,7 +137,7 @@ public class RobotContainer {
 
         /* Storage Neo PID Test */
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1)));
+            .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5)));
     }
       
     /**
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 5fbcd1c..be3f97f 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -19,7 +19,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
+import frc4388.robot.Gains;
 import frc4388.robot.Constants.StorageConstants;
 
 public class Storage extends SubsystemBase {
@@ -30,6 +30,8 @@ public class Storage extends SubsystemBase {
 
   CANEncoder m_encoder = m_storageMotor.getEncoder();
 
+  Gains storageGains = StorageConstants.STORAGE_GAINS;
+
   /**
    * Creates a new Storage.
    */
@@ -72,15 +74,15 @@ public class Storage extends SubsystemBase {
   }
 
   /* Storage PID Control */
-  public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput)
+  public void runStoragePositionPID(double targetPos)
   {
     // Set PID Coefficients
-    m_storagePIDController.setP(kP);
-    m_storagePIDController.setI(kI);
-    m_storagePIDController.setD(kD);
-    m_storagePIDController.setIZone(kIz);
-    m_storagePIDController.setFF(kF);
-    m_storagePIDController.setOutputRange(kminOutput, kmaxOutput);
+    m_storagePIDController.setP(storageGains.m_kP);
+    m_storagePIDController.setI(storageGains.m_kI);
+    m_storagePIDController.setD(storageGains.m_kD);
+    m_storagePIDController.setIZone(storageGains.m_kIzone);
+    m_storagePIDController.setFF(storageGains.m_kF);
+    m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kPeakOutput);
 
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
@@ -94,7 +96,7 @@ public class Storage extends SubsystemBase {
    * Prepares storage for shooting
    */
   public void storageAim() {
-
+    //runStoragePositionPID(targetPos, kP, kI, kD, kIz, kF, kmaxOutput, kminOutput);
   }
     /*
     *If shooting move storage motor until top sensor is tripped

From 1463bd209472d311be0b74d078e9ad772c0fde3b Mon Sep 17 00:00:00 2001
From: Rishabh <57200@psdschools.org>
Date: Thu, 13 Feb 2020 19:07:36 -0700
Subject: [PATCH 03/31] Add up and down for storage thingy

---
 .../java/frc4388/robot/RobotContainer.java    |  3 +++
 .../frc4388/robot/subsystems/Storage.java     | 19 +++++++++++++++++--
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index bf4a506..5ad26e8 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -138,6 +138,9 @@ public class RobotContainer {
         /* Storage Neo PID Test */
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5)));
+
+        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
+            .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake()));
     }
       
     /**
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index be3f97f..5f4d610 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -65,7 +65,7 @@ public class Storage extends SubsystemBase {
     } else {
       System.err.println("Beam off");
     }
-
+    
   }
 
   public void resetEncoder()
@@ -96,7 +96,21 @@ public class Storage extends SubsystemBase {
    * Prepares storage for shooting
    */
   public void storageAim() {
-    //runStoragePositionPID(targetPos, kP, kI, kD, kIz, kF, kmaxOutput, kminOutput);
+    if (m_beamSensors[0].get() == false){
+      m_storageMotor.set(0.5);
+    }
+    else{
+      m_storageMotor.set(0);
+    }
+  }
+
+public void storageIntake() {
+  if (m_beamSensors[2].get() == false){
+    m_storageMotor.set(-0.5);
+  }
+  else{
+    m_storageMotor.set(0);
+
   }
     /*
     *If shooting move storage motor until top sensor is tripped
@@ -104,3 +118,4 @@ public class Storage extends SubsystemBase {
     *
     */
 }
+}

From 0cf61e189337aa233ee5dc2d2041b3c52ad4dbc7 Mon Sep 17 00:00:00 2001
From: Sebastian <66741@psdschools.org>
Date: Sat, 15 Feb 2020 11:16:12 -0800
Subject: [PATCH 04/31] Update Storage.java

---
 .../frc4388/robot/subsystems/Storage.java     | 33 +++++++++----------
 1 file changed, 16 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 5f4d610..280b3f9 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -32,6 +32,8 @@ public class Storage extends SubsystemBase {
 
   Gains storageGains = StorageConstants.STORAGE_GAINS;
 
+  Intake m_intake;
+
   /**
    * Creates a new Storage.
    */
@@ -56,17 +58,7 @@ public class Storage extends SubsystemBase {
    * 
    * @param input the voltage to run motor at
    */
-  public void runStorage(final double input) {
-    m_storageMotor.set(input);
-    final boolean beam_on = m_beamSensors[0].get();
-
-    if (beam_on) {
-      System.err.println("Beam on");
-    } else {
-      System.err.println("Beam off");
-    }
-    
-  }
+  
 
   public void resetEncoder()
   {
@@ -95,8 +87,8 @@ public class Storage extends SubsystemBase {
   /**
    * Prepares storage for shooting
    */
-  public void storageAim() {
-    if (m_beamSensors[0].get() == false){
+  public void storageAim() {  
+    if (m_beamSensors[2].get() == false){
       m_storageMotor.set(0.5);
     }
     else{
@@ -104,13 +96,21 @@ public class Storage extends SubsystemBase {
     }
   }
 
-public void storageIntake() {
-  if (m_beamSensors[2].get() == false){
+public void storageIntake(Intake intake) {
+  m_intake = intake;
+  if (m_beamSensors[1].get() == false){
     m_storageMotor.set(-0.5);
   }
   else{
     m_storageMotor.set(0);
-
+  }
+  if (m_beamSensors[0].get()){
+    m_intake.runExtender(-0.3);
+    m_storagePIDController.setReference(10, ControlType.kPosition);
+  }
+}
+public void storageOuttake() {
+     m_storageMotor.set(1);
   }
     /*
     *If shooting move storage motor until top sensor is tripped
@@ -118,4 +118,3 @@ public void storageIntake() {
     *
     */
 }
-}

From 57cc8a26e6fa921cb851a98332a710fb7bc1409b Mon Sep 17 00:00:00 2001
From: Sebastian <66741@psdschools.org>
Date: Sat, 15 Feb 2020 16:01:28 -0800
Subject: [PATCH 05/31] Even More Janky Code

Storage
---
 src/main/java/frc4388/robot/RobotContainer.java   | 10 +++++++---
 .../java/frc4388/robot/subsystems/Intake.java     | 12 ++++++++----
 .../java/frc4388/robot/subsystems/Storage.java    | 15 ++++++++-------
 3 files changed, 23 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 5ad26e8..389c553 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -79,9 +79,8 @@ public class RobotContainer {
         m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter));
         // drives the leveler with an axis input from the driver controller
         m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
-        // runs storage motor at 50 percent
-        m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
     }
+      
 
     /**
     * Use this method to define your button->command mappings.  Buttons can be created by
@@ -139,8 +138,13 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5)));
 
+        //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
-            .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake()));
+            .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake(m_robotIntake)));
+            
+        //Runs storage to outtake
+        new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
+            .whileHeld(new RunCommand(() -> m_robotStorage.storageOuttake()));
     }
       
     /**
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 62bb30a..679aac3 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -17,16 +17,20 @@ import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.IntakeConstants;
 
+
+
 public class Intake extends SubsystemBase {
   CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
   CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
-  CANDigitalInput m_extenderForwardLimit;
-  CANDigitalInput m_extenderReverseLimit;
+
 
   /**
    * Creates a new Intake.
    */
   public Intake() {
+    CANDigitalInput m_extenderForwardLimit;
+    CANDigitalInput m_extenderReverseLimit;
+
     m_intakeMotor.restoreFactoryDefaults();
     m_extenderMotor.restoreFactoryDefaults();
 
@@ -50,7 +54,7 @@ public class Intake extends SubsystemBase {
    * Runs intake motor
    * @param input the percent output to run motor at
    */
-  public void runIntake(double input) {
+  public void runIntake(final double input) {
     m_intakeMotor.set(input);
   }
   
@@ -58,7 +62,7 @@ public class Intake extends SubsystemBase {
    * Runs extender motor
    * @param input the percent output to run motor at
    */
-  public void runExtender(double input) {
+  public void runExtender(final double input) {
     m_extenderMotor.set(input);
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 280b3f9..7c35066 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -9,12 +9,14 @@ package frc4388.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.ControlType;
 import com.revrobotics.SparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
@@ -95,7 +97,6 @@ public class Storage extends SubsystemBase {
       m_storageMotor.set(0);
     }
   }
-
 public void storageIntake(Intake intake) {
   m_intake = intake;
   if (m_beamSensors[1].get() == false){
@@ -110,11 +111,11 @@ public void storageIntake(Intake intake) {
   }
 }
 public void storageOuttake() {
-     m_storageMotor.set(1);
+  m_storageMotor.set(1);
+  
+  /*
+  *If shooting move storage motor until top sensor is tripped
+  *If intaking move storage motor until bottom sensor is tripped
+  */
   }
-    /*
-    *If shooting move storage motor until top sensor is tripped
-    *If intaking move storage motor until bottom sensor is tripped
-    *
-    */
 }

From 86d6d5d946c548da2510d4458e1817f90bd6fe65 Mon Sep 17 00:00:00 2001
From: Sebastian <66741@psdschools.org>
Date: Tue, 18 Feb 2020 18:12:06 -0800
Subject: [PATCH 06/31] Limit switches for extenders

Co-Authored-By: llamakingker <llamakingker@users.noreply.github.com>

#janky
---
 .../java/frc4388/robot/subsystems/Intake.java | 23 +++++++++++++++----
 .../frc4388/robot/subsystems/Storage.java     |  2 +-
 2 files changed, 19 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 679aac3..63cb587 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -22,14 +22,15 @@ import frc4388.robot.Constants.IntakeConstants;
 public class Intake extends SubsystemBase {
   CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
   CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
-
+  CANDigitalInput m_extenderForwardLimit;
+  CANDigitalInput m_extenderReverseLimit;
+  boolean extended = false;
 
   /**
    * Creates a new Intake.
    */
   public Intake() {
-    CANDigitalInput m_extenderForwardLimit;
-    CANDigitalInput m_extenderReverseLimit;
+    
 
     m_intakeMotor.restoreFactoryDefaults();
     m_extenderMotor.restoreFactoryDefaults();
@@ -63,6 +64,18 @@ public class Intake extends SubsystemBase {
    * @param input the percent output to run motor at
    */
   public void runExtender(final double input) {
-    m_extenderMotor.set(input);
+    if (m_extenderForwardLimit.get()) {
+      extended = true;
+    }
+    if (m_extenderReverseLimit.get()) {
+      extended = false;
+    }
+    
+    if (extended == false) {
+      m_extenderMotor.set(0.5);
+    }
+    if (extended == true) {
+      m_extenderMotor.set(-0.5);
+    }
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 7c35066..db3d2fe 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -106,7 +106,7 @@ public void storageIntake(Intake intake) {
     m_storageMotor.set(0);
   }
   if (m_beamSensors[0].get()){
-    m_intake.runExtender(-0.3);
+    m_intake.runExtender(-0.3); 
     m_storagePIDController.setReference(10, ControlType.kPosition);
   }
 }

From d89f9da745ce7ab33e0bc3f7defb5c0b8488d084 Mon Sep 17 00:00:00 2001
From: Sebastian <66741@psdschools.org>
Date: Tue, 18 Feb 2020 18:45:07 -0800
Subject: [PATCH 07/31] Changed Limit Switch on Extender

Co-Authored-By: llamakingker <llamakingker@users.noreply.github.com>

#janky
---
 .../robot/commands/RunExtenderOutIn.java      | 30 ++++++++++++++++---
 1 file changed, 26 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
index b0bb140..6cfd9c4 100644
--- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
+++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
@@ -7,14 +7,25 @@
 
 package frc4388.robot.commands;
 
+import com.revrobotics.CANDigitalInput;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.subsystems.Intake;
 import frc4388.utility.controller.IHandController;
 
 public class RunExtenderOutIn extends CommandBase {
   private Intake m_intake;
   private boolean isOut = false;
-  private long startTime;
+  CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
+  CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
+  CANDigitalInput m_extenderForwardLimit;
+  CANDigitalInput m_extenderReverseLimit;
+
+  
 
   /**
    * Uses input from opperator to run the extender motor.
@@ -25,13 +36,17 @@ public class RunExtenderOutIn extends CommandBase {
     // Use addRequirements() here to declare subsystem dependencies.
     m_intake = subsystem;
     addRequirements(m_intake);
+
+    m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
+    m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
+    m_extenderForwardLimit.enableLimitSwitch(false);
+    m_extenderReverseLimit.enableLimitSwitch(false);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
     isOut = !isOut;
-    startTime = System.currentTimeMillis();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -54,9 +69,16 @@ public class RunExtenderOutIn extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (startTime + 3000 < System.currentTimeMillis()) {
+    if (isOut && m_extenderForwardLimit.get() == true){
       return true;
     }
-    return false;
+    
+    else if(isOut && m_extenderReverseLimit.get() == true){
+      return true;
+    }
+    
+    else{
+      return false;
+    }
   }
 }

From b0f420dc692cb06998c485971a219316b67ee9dc Mon Sep 17 00:00:00 2001
From: Kerem <81587@psdschools.org>
Date: Tue, 18 Feb 2020 19:56:03 -0700
Subject: [PATCH 08/31] Added buttons

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 50cedc1..0a735c2 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -82,7 +82,6 @@ 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 drum shooter in idle mode
-
         m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
         // drives the leveler with an axis input from the driver controller
     //    m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
@@ -110,10 +109,10 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
             .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000));
             
-        new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
+        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new TrackTarget(m_robotShooter));
         
-        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
+        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
             .whenPressed(new RunExtenderOutIn(m_robotIntake));
       
         /* PID Test Command */
@@ -135,11 +134,11 @@ public class RobotContainer {
 
 
         // sets solenoids into high gear
-        new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON)
+        new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
 
         // sets solenoids into low gear
-        new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
+        new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
         
         // interrupts any running command

From 6742028ba9e742587f01e16be5f715004c6d13c0 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Wed, 19 Feb 2020 20:48:34 -0700
Subject: [PATCH 09/31] Add Shooting capabilities

---
 src/main/java/frc4388/robot/Constants.java    |  1 +
 .../java/frc4388/robot/RobotContainer.java    |  8 +-
 .../frc4388/robot/commands/ShootShooter.java  | 75 +++++++++++++++++++
 .../frc4388/robot/commands/TrackTarget.java   | 16 ++++
 4 files changed, 98 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/ShootShooter.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index c4e5917..a1a5901 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -137,6 +137,7 @@ public final class Constants {
         public static final double MOTOR_DEAD_ZONE = 0.3;
         public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
         public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;
+        public static final double GRAV = 385.83;
         }
 
     public static final class OIConstants {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 0a735c2..493ab99 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -30,6 +30,7 @@ import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
+import frc4388.robot.commands.ShootShooter;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
@@ -106,8 +107,11 @@ public class RobotContainer {
         //    .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         //    .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
       
-        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-            .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000));
+        new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
+            .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
+
+        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
+            .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
             
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new TrackTarget(m_robotShooter));
diff --git a/src/main/java/frc4388/robot/commands/ShootShooter.java b/src/main/java/frc4388/robot/commands/ShootShooter.java
new file mode 100644
index 0000000..370c4b5
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ShootShooter.java
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.commands.TrackTarget;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.Storage;
+
+public class ShootShooter extends CommandBase {
+  Shooter m_shooter;
+  Storage m_storage;
+  private long startTime;
+  private int ballNum;
+  /**
+   * Creates a new ShootAlll.
+   */
+  public ShootShooter(Shooter shootSub, Storage storeSub, int numBall) {
+    ballNum = numBall;
+    m_shooter = shootSub;
+    m_storage = storeSub;
+    addRequirements(m_shooter);
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    startTime = System.currentTimeMillis();
+    //new InstantCommand(() -> m_storage.storageAiming());
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    TrackTarget track = new TrackTarget(m_shooter); //Used to pull velocity and angle from TrackTarget.java
+
+      //Aiming
+    if (startTime + 3000 > System.currentTimeMillis())
+    {
+      new ShooterVelocityControlPID(m_shooter, track.addFireVel());
+    }
+    
+    else if(3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){
+      new RunCommand(() -> m_storage.runStorage(0.5));
+      new ShooterVelocityControlPID(m_shooter, track.addFireVel());
+      new RunCommand(() -> m_shooter.runAngleAdjustPID(track.addFireAngle()));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 2853242..3f806d4 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Shooter;
@@ -28,6 +29,8 @@ public class TrackTarget extends CommandBase {
   double yAngle = 0;
   double target = 0;
   public double distance;
+  public static double fireVel;
+  public static double fireAngle;
 
   /**
    * Uses the Limelight to track the target
@@ -46,6 +49,13 @@ public class TrackTarget extends CommandBase {
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
+  public double addFireVel() {
+    return fireVel;
+  }
+  public double addFireAngle() {
+    return fireAngle;
+  }
+
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
@@ -65,6 +75,12 @@ public class TrackTarget extends CommandBase {
         //Finding Distance
       distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
       SmartDashboard.putNumber("Distance to Target", distance);
+
+      double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
+      double xVel = (distance*VisionConstants.GRAV)/(yVel);
+
+      fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
+      fireAngle = Math.atan(yVel/xVel);
     }
   }
 

From 7667182b012dd0b87299ae6d142c50370528536e Mon Sep 17 00:00:00 2001
From: Aarav Shah <91212@psdschools.org>
Date: Thu, 20 Feb 2020 17:00:50 -0700
Subject: [PATCH 10/31] Changes to isExtended boolean

Refactored name according to boolean convention and combined two different booleans doing the same thing.
---
 .../java/frc4388/robot/commands/RunExtenderOutIn.java  | 10 +++++-----
 src/main/java/frc4388/robot/subsystems/Intake.java     | 10 +++++-----
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
index 6cfd9c4..3c062a0 100644
--- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
+++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java
@@ -19,7 +19,7 @@ import frc4388.utility.controller.IHandController;
 
 public class RunExtenderOutIn extends CommandBase {
   private Intake m_intake;
-  private boolean isOut = false;
+
   CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
   CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
   CANDigitalInput m_extenderForwardLimit;
@@ -46,13 +46,13 @@ public class RunExtenderOutIn extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    isOut = !isOut;
+    m_intake.isExtended = !m_intake.isExtended;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (isOut){
+    if (m_intake.isExtended){
       m_intake.runExtender(0.3);
     } else {
       m_intake.runExtender(-0.3);
@@ -69,11 +69,11 @@ public class RunExtenderOutIn extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (isOut && m_extenderForwardLimit.get() == true){
+    if (m_intake.isExtended && m_extenderForwardLimit.get() == true){
       return true;
     }
     
-    else if(isOut && m_extenderReverseLimit.get() == true){
+    else if(m_intake.isExtended && m_extenderReverseLimit.get() == true){
       return true;
     }
     
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 63cb587..d6b9b32 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -24,7 +24,7 @@ public class Intake extends SubsystemBase {
   CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
   CANDigitalInput m_extenderForwardLimit;
   CANDigitalInput m_extenderReverseLimit;
-  boolean extended = false;
+  public boolean isExtended = false;
 
   /**
    * Creates a new Intake.
@@ -65,16 +65,16 @@ public class Intake extends SubsystemBase {
    */
   public void runExtender(final double input) {
     if (m_extenderForwardLimit.get()) {
-      extended = true;
+      isExtended = true;
     }
     if (m_extenderReverseLimit.get()) {
-      extended = false;
+      isExtended = false;
     }
     
-    if (extended == false) {
+    if (isExtended == false) {
       m_extenderMotor.set(0.5);
     }
-    if (extended == true) {
+    if (isExtended == true) {
       m_extenderMotor.set(-0.5);
     }
   }

From 01a0327cf3fe4f321b5f7eaf98a5ce0cc832287c Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Thu, 20 Feb 2020 20:21:28 -0700
Subject: [PATCH 11/31] Try to make shooter work

---
 src/main/java/frc4388/robot/Constants.java    |  7 +----
 .../frc4388/robot/commands/ShootShooter.java  | 28 +++++++++----------
 .../frc4388/robot/commands/TrackTarget.java   |  9 ++----
 .../frc4388/robot/subsystems/Shooter.java     | 23 ++++++++++++++-
 .../frc4388/robot/subsystems/Storage.java     | 22 ++++++++-------
 5 files changed, 52 insertions(+), 37 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index a1a5901..3f9f627 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -114,13 +114,8 @@ public final class Constants {
         public static final int PID_PRIMARY = 0;
 
         /* PID Gains */
-        public static final double storP = 0.1;
-        public static final double storI = 1e-4;
-        public static final double storD = 1.0;
-        public static final double storIz = 0.0;
-        public static final double storF = 0.0;
-        public static final double storkmaxOutput = 1.0;
         public static final double storkminOutput = -1.0;
+        public static final Gains STORAGE_GAINS = new Gains(0.1, 0, 1, 0, 0, 1);
     }
   
     public static final class LEDConstants {
diff --git a/src/main/java/frc4388/robot/commands/ShootShooter.java b/src/main/java/frc4388/robot/commands/ShootShooter.java
index 370c4b5..d9b2d1f 100644
--- a/src/main/java/frc4388/robot/commands/ShootShooter.java
+++ b/src/main/java/frc4388/robot/commands/ShootShooter.java
@@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.Constants.ShooterConstants;
-import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Storage;
 
@@ -23,6 +22,7 @@ public class ShootShooter extends CommandBase {
   Storage m_storage;
   private long startTime;
   private int ballNum;
+  public boolean velReach = false;
   /**
    * Creates a new ShootAlll.
    */
@@ -30,35 +30,35 @@ public class ShootShooter extends CommandBase {
     ballNum = numBall;
     m_shooter = shootSub;
     m_storage = storeSub;
-    addRequirements(m_shooter);
-    addRequirements(m_storage);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    startTime = System.currentTimeMillis();
-    //new InstantCommand(() -> m_storage.storageAiming());
+    //new InstantCommand(() -> m_storage.storageAim());
+    velReach = false;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    TrackTarget track = new TrackTarget(m_shooter); //Used to pull velocity and angle from TrackTarget.java
 
       //Aiming
-    if (startTime + 3000 > System.currentTimeMillis())
+    if (!m_shooter.velReached && velReach == false) //If the shooter is spooled
     {
-      new ShooterVelocityControlPID(m_shooter, track.addFireVel());
+      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel());
+      startTime = System.currentTimeMillis();
     }
     
-    else if(3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){
-      new RunCommand(() -> m_storage.runStorage(0.5));
-      new ShooterVelocityControlPID(m_shooter, track.addFireVel());
-      new RunCommand(() -> m_shooter.runAngleAdjustPID(track.addFireAngle()));
+    else {
+      velReach = true;
+      new ParallelCommandGroup(
+        new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
+        new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
+        new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + (2*ballNum)))
+      );
     }
   }
-
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
@@ -67,7 +67,7 @@ public class ShootShooter extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){
+    if (startTime + (1000 * ballNum) < System.currentTimeMillis()){
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 3f806d4..8102f13 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -49,12 +49,7 @@ public class TrackTarget extends CommandBase {
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
-  public double addFireVel() {
-    return fireVel;
-  }
-  public double addFireAngle() {
-    return fireAngle;
-  }
+
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -81,6 +76,8 @@ public class TrackTarget extends CommandBase {
 
       fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
       fireAngle = Math.atan(yVel/xVel);
+      m_shooter.m_fireVel = fireVel;
+      m_shooter.m_fireAngle = fireAngle;
     }
   }
 
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index a18aca5..c4487ce 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -48,6 +48,9 @@ public class Shooter extends SubsystemBase {
 
   double velP;
   double input;
+  public boolean velReached;
+  public double m_fireVel;
+  public double m_fireAngle;
   
   /*
    * Creates a new Shooter subsystem.
@@ -79,6 +82,13 @@ public class Shooter extends SubsystemBase {
     SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
   }
 
+  public double addFireVel() {
+    return m_fireVel;
+  }
+  public double addFireAngle() {
+    return m_fireAngle;
+  }
+
   /**
    * Runs drum shooter motor.
    * @param speed Speed to set the motor at
@@ -105,15 +115,26 @@ public class Shooter extends SubsystemBase {
   public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
     velP = actualVel/targetVel; //Percent of target
     double runSpeed = actualVel + (12000*velP); //Ramp up equation
-    //if (runSpeed > targetVel) {runSpeed = targetVel;}
     SmartDashboard.putNumber("shoot", actualVel);
     runSpeed = runSpeed/targetVel; //Convert to percent
+
     if (actualVel < targetVel - 1000){ //PID Based on ramp up amount
       m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed);
     }
     else{ //PID Based on targetVel
       m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
     }
+
+      //Tells wether the target velocity has been reached
+    double upperBound = targetVel + 300;
+    double lowerBound = targetVel - 300;
+    if (actualVel < upperBound && actualVel > lowerBound)
+    {
+      velReached = true;
+    }
+    else{
+      velReached = false;
+    }
   }
 
 
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 790e59e..50628c6 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -18,7 +18,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
+import frc4388.robot.Gains;
 import frc4388.robot.Constants.StorageConstants;
 
 public class Storage extends SubsystemBase {
@@ -29,6 +29,8 @@ public class Storage extends SubsystemBase {
 
   CANEncoder m_encoder = m_storageMotor.getEncoder();
 
+  public static Gains m_storageGains = StorageConstants.STORAGE_GAINS;
+
   /**
    * Creates a new Storage.
    */
@@ -65,21 +67,21 @@ public class Storage extends SubsystemBase {
   }
 
   /* Storage PID Control */
-  public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput)
+  public void runStoragePositionPID(double targetPos)
   {
     // Set PID Coefficients
-    m_storagePIDController.setP(kP);
-    m_storagePIDController.setI(kI);
-    m_storagePIDController.setD(kD);
-    m_storagePIDController.setIZone(kIz);
-    m_storagePIDController.setFF(kF);
-    m_storagePIDController.setOutputRange(kminOutput, kmaxOutput);
+    m_storagePIDController.setP(m_storageGains.m_kP);
+    m_storagePIDController.setI(m_storageGains.m_kI);
+    m_storagePIDController.setD(m_storageGains.m_kD);
+    m_storagePIDController.setIZone(m_storageGains.m_kIzone);
+    m_storagePIDController.setFF(m_storageGains.m_kF);
+    m_storagePIDController.setOutputRange(StorageConstants.storkminOutput, m_storageGains.m_kmaxOutput);
 
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
 
-  public void getEncoderPos()
+  public double getEncoderPos()
   {
-    m_encoder.getPosition();
+    return m_encoder.getPosition();
   }
 }

From 7cdeb09908a6a89854045d321e7f4482feb7e5bc Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 16:59:05 -0700
Subject: [PATCH 12/31] TODO Shooter

---
 .../java/frc4388/robot/RobotContainer.java    |  9 +--
 .../frc4388/robot/commands/ShootShooter.java  | 75 -------------------
 2 files changed, 4 insertions(+), 80 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/commands/ShootShooter.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 493ab99..77dc2a9 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -30,7 +30,6 @@ import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
-import frc4388.robot.commands.ShootShooter;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
@@ -107,11 +106,11 @@ public class RobotContainer {
         //    .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         //    .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
       
-        new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
+        //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
+        //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
 
-        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
+        //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
+        //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
             
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new TrackTarget(m_robotShooter));
diff --git a/src/main/java/frc4388/robot/commands/ShootShooter.java b/src/main/java/frc4388/robot/commands/ShootShooter.java
deleted file mode 100644
index d9b2d1f..0000000
--- a/src/main/java/frc4388/robot/commands/ShootShooter.java
+++ /dev/null
@@ -1,75 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.wpilibj.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc4388.robot.Constants.ShooterConstants;
-import frc4388.robot.subsystems.Shooter;
-import frc4388.robot.subsystems.Storage;
-
-public class ShootShooter extends CommandBase {
-  Shooter m_shooter;
-  Storage m_storage;
-  private long startTime;
-  private int ballNum;
-  public boolean velReach = false;
-  /**
-   * Creates a new ShootAlll.
-   */
-  public ShootShooter(Shooter shootSub, Storage storeSub, int numBall) {
-    ballNum = numBall;
-    m_shooter = shootSub;
-    m_storage = storeSub;
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    //new InstantCommand(() -> m_storage.storageAim());
-    velReach = false;
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-
-      //Aiming
-    if (!m_shooter.velReached && velReach == false) //If the shooter is spooled
-    {
-      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel());
-      startTime = System.currentTimeMillis();
-    }
-    
-    else {
-      velReach = true;
-      new ParallelCommandGroup(
-        new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
-        new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
-        new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + (2*ballNum)))
-      );
-    }
-  }
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    if (startTime + (1000 * ballNum) < System.currentTimeMillis()){
-      return true;
-    }
-    return false;
-  }
-}

From e7e91d9c4cde502ca53414e8a4b412c74a48c943 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 17:02:21 -0700
Subject: [PATCH 13/31] add thing for later

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 77dc2a9..3015bac 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -113,7 +113,8 @@ public class RobotContainer {
         //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
             
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter));
+            .whileHeld(new TrackTarget(m_robotShooter))
+            //.whileHeld(new RunCommand(() -> m_robotStorage.storeAim()));
         
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
             .whenPressed(new RunExtenderOutIn(m_robotIntake));

From 84c83dd3e56c824540879bcda5357aecbf7c6e6f Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 17:16:16 -0700
Subject: [PATCH 14/31] Fixed gradle fail

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 3015bac..293123e 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -113,7 +113,7 @@ public class RobotContainer {
         //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
             
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter))
+            .whileHeld(new TrackTarget(m_robotShooter));
             //.whileHeld(new RunCommand(() -> m_robotStorage.storeAim()));
         
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -154,9 +154,6 @@ public class RobotContainer {
             .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
             .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
 
-        /* Storage Neo PID Test */
-        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter));
     }
       
     /**

From 9983b9354b5bb105e60a49bac9b776e95fbd6b9d Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 17:25:17 -0700
Subject: [PATCH 15/31] FIX

---
 .../java/frc4388/robot/RobotContainer.java    | 62 ++++++-------------
 1 file changed, 20 insertions(+), 42 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 4d5a858..265f57b 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -112,46 +112,7 @@ public class RobotContainer {
     */
     private void configureButtonBindings() {
         /* Driver Buttons */
-        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive));
-
-        /* Operator Buttons */
-        // activates "Lit Mode"
-        //new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-        //    .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-        //    .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
-      
-        //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-        //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
-
-        //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-        //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
-            
-        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter));
-            //.whileHeld(new RunCommand(() -> m_robotStorage.storeAim()));
         
-        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-            .whenPressed(new RunExtenderOutIn(m_robotIntake));
-      
-        /* PID Test Command */
-        // runs velocity PID while driving straight
-        new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500))
-            .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
-        
-        //new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-        //    .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController()));
-      
-        // resets the yaw of the pigeon
-        new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive));
-      
-        // turn 45 degrees
-        new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive));
-
-
         // sets solenoids into high gear
         new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
@@ -159,10 +120,27 @@ public class RobotContainer {
         // sets solenoids into low gear
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
+
+
+        /* Operator Buttons */
+      
+            //TODO: Shooter Buttons
+        // shoots until released
+        //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
+        //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
+
+        // shoots one ball
+        //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
+        //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
+            
+        // aims the turret
+        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+            .whileHeld(new TrackTarget(m_robotShooter));
+            //.whenPressed(new RunCommand(() -> m_robotStorage.storeAim()));
         
-        // interrupts any running command
-        new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
-            .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
+        // extends or retracts the extender
+        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
+            .whenPressed(new RunExtenderOutIn(m_robotIntake));
 
         // safety for climber and leveler
         new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)

From 629046c0329094a6373f70ecb4ee3e2f8b006b44 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 20:41:36 -0700
Subject: [PATCH 16/31] aaaaaaaaaaaa

---
 .../java/frc4388/robot/RobotContainer.java    |  6 +-
 .../robot/commands/StorageIntakeFinal.java    | 46 +++++++++++++++
 .../robot/commands/StorageIntakeGroup.java    | 27 +++++++++
 .../frc4388/robot/commands/storageIntake.java | 58 +++++++++++++++++++
 .../frc4388/robot/commands/storageOutake.java | 44 ++++++++++++++
 .../robot/commands/storagePrepAim.java        | 52 +++++++++++++++++
 .../robot/commands/storagePrepIntake.java     | 56 ++++++++++++++++++
 .../frc4388/robot/subsystems/Storage.java     | 51 +++++-----------
 8 files changed, 300 insertions(+), 40 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
 create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
 create mode 100644 src/main/java/frc4388/robot/commands/storageIntake.java
 create mode 100644 src/main/java/frc4388/robot/commands/storageOutake.java
 create mode 100644 src/main/java/frc4388/robot/commands/storagePrepAim.java
 create mode 100644 src/main/java/frc4388/robot/commands/storagePrepIntake.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 26d1a69..ac85461 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -39,6 +39,7 @@ import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
 import frc4388.robot.commands.RunIntakeWithTriggers;
 import frc4388.robot.commands.ShooterVelocityControlPID;
+import frc4388.robot.commands.StorageIntakeGroup;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
@@ -49,6 +50,7 @@ import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.commands.TrackTarget;
+import frc4388.robot.commands.storageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.Storage;
@@ -171,11 +173,11 @@ public class RobotContainer {
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
-            .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake(m_robotIntake)));
+            .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage));
             
         //Runs storage to outtake
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
-            .whileHeld(new RunCommand(() -> m_robotStorage.storageOuttake()));
+            .whileHeld(new storageOutake(m_robotStorage));
     }
        
     /**
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
new file mode 100644
index 0000000..9576f35
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandBase;
+import frc4388.robot.subsystems.Storage;
+
+public class StorageIntakeFinal extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new StorageIntakeFinal.
+   */
+  public StorageIntakeFinal(Storage subsystem) {
+    m_storage = subsystem;
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (m_storage.getBeam(1)){
+      m_storage.setStoragePID(m_storage.getEncoderPos() + 5);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
new file mode 100644
index 0000000..767ed69
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.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;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Storage;
+
+// 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 StorageIntakeGroup extends SequentialCommandGroup {
+  /**
+   * Creates a new StorageIntakeGroup.
+   */
+  public StorageIntakeGroup(Intake m_intake, Storage m_storage) {
+    addCommands(
+      new storagePrepIntake(m_intake, m_storage), 
+      new storageIntake(m_intake, m_storage), 
+      new StorageIntakeFinal(m_storage));
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/storageIntake.java b/src/main/java/frc4388/robot/commands/storageIntake.java
new file mode 100644
index 0000000..ee30708
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storageIntake.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandBase;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Storage;
+
+public class storageIntake extends CommandBase {
+  public Intake m_intake;
+  public Storage m_storage;
+  /**
+   * Creates a new storageIntake.
+   */
+  public storageIntake(Intake inSub, Storage storeSub) {
+    m_intake = inSub;
+    m_storage = storeSub;
+    addRequirements(m_intake);
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+    if (m_storage.getBeam(0)){
+      m_storage.setStoragePID(m_storage.getEncoderPos() + 2);
+      m_intake.runExtender(-0.3); 
+    }
+    else{
+      m_intake.runExtender(0.3); 
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (m_storage.getBeam(0)){
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/storageOutake.java
new file mode 100644
index 0000000..4820dc0
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storageOutake.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandBase;
+import frc4388.robot.subsystems.Storage;
+
+public class storageOutake extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new storageOutake.
+   */
+  public storageOutake(Storage storeSub) {
+    m_storage = storeSub;
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      m_storage.runStorage(-0.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/storagePrepAim.java b/src/main/java/frc4388/robot/commands/storagePrepAim.java
new file mode 100644
index 0000000..c8b3bad
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storagePrepAim.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandBase;
+import frc4388.robot.subsystems.Storage;
+
+public class storagePrepAim extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new storagePrepAim.
+   */
+  public storagePrepAim(Storage storeSub) {
+    m_storage = storeSub;
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (m_storage.getBeam(2) == false){
+      m_storage.runStorage(0.5);
+    }
+    else{
+      m_storage.runStorage(0);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (m_storage.getBeam(2)){
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
new file mode 100644
index 0000000..7fab016
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandBase;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Storage;
+
+public class storagePrepIntake extends CommandBase {
+  public Intake m_intake;
+  public Storage m_storage;
+  /**
+   * Creates a new storagePrepIntake.
+   */
+  public storagePrepIntake(Intake inSub, Storage storeSub) {
+    m_intake = inSub;
+    m_storage = storeSub;
+    addRequirements(m_intake);
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (m_storage.getBeam(1) == false){
+      m_storage.runStorage(-0.5);
+    }
+    else{
+      m_storage.runStorage(0);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (m_storage.getBeam(1)){
+      return true;
+    }
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index c71355a..24422c9 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -19,6 +19,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.StorageConstants;
@@ -35,12 +36,14 @@ public class Storage extends SubsystemBase {
 
   Intake m_intake;
 
+  public boolean botReached;
+
   /**
    * Creates a new Storage.
    */
   public Storage() {
     resetEncoder();
-
+    boolean botReached = false;
     m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0);
     m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1);
     m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2);
@@ -65,14 +68,12 @@ public class Storage extends SubsystemBase {
     final boolean beam_on = m_beamSensors[0].get();
   }
 
-  public void resetEncoder()
-  {
+  public void resetEncoder(){
     m_encoder.setPosition(0);
   }
 
   /* Storage PID Control */
-  public void runStoragePositionPID(double targetPos)
-  {
+  public void runStoragePositionPID(double targetPos){
     // Set PID Coefficients
     m_storagePIDController.setP(storageGains.m_kP);
     m_storagePIDController.setI(storageGains.m_kI);
@@ -84,41 +85,15 @@ public class Storage extends SubsystemBase {
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
 
-  public void getEncoderPos()
-  {
-    m_encoder.getPosition();
+  public double getEncoderPos(){
+    return m_encoder.getPosition();
   }
 
-  /**
-   * Prepares storage for shooting
-   */
-  public void storageAim() {  
-    if (m_beamSensors[2].get() == false){
-      m_storageMotor.set(0.5);
-    }
-    else{
-      m_storageMotor.set(0);
-    }
+  public boolean getBeam(int id){
+    return m_beamSensors[id].get();
   }
-public void storageIntake(Intake intake) {
-  m_intake = intake;
-  if (m_beamSensors[1].get() == false){
-    m_storageMotor.set(-0.5);
-  }
-  else{
-    m_storageMotor.set(0);
-  }
-  if (m_beamSensors[0].get()){
-    m_intake.runExtender(-0.3); 
-    m_storagePIDController.setReference(10, ControlType.kPosition);
-  }
-}
-public void storageOuttake() {
-  m_storageMotor.set(1);
-  
-  /*
-  *If shooting move storage motor until top sensor is tripped
-  *If intaking move storage motor until bottom sensor is tripped
-  */
+
+  public void setStoragePID(double position){
+    m_storagePIDController.setReference(position , ControlType.kPosition);
   }
 }

From 5f989fb21a67c9519d27ba126a04ee17d3cc065d Mon Sep 17 00:00:00 2001
From: Aarav Shah <91212@psdschools.org>
Date: Fri, 21 Feb 2020 20:46:52 -0700
Subject: [PATCH 17/31] Tweaks to Deadassist and Work on Motion Magic

---
 src/main/java/frc4388/robot/Constants.java    | 10 ++++-----
 .../java/frc4388/robot/RobotContainer.java    |  2 +-
 .../commands/DriveStraightToPositionMM.java   |  7 ++++++-
 .../robot/commands/DriveWithJoystick.java     |  2 +-
 .../DriveWithJoystickUsingDeadAssistPID.java  |  8 +++----
 .../java/frc4388/robot/subsystems/Drive.java  | 21 ++++++++++---------
 6 files changed, 27 insertions(+), 23 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 54636c2..1de9634 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -29,12 +29,10 @@ public final class Constants {
 
         /* PID Constants Drive*/
         public static final int DRIVE_TIMEOUT_MS = 30;
-        public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3);
-        public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.45);
-        public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.45);
-        //public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
-        //public static final int DRIVE_CRUISE_VELOCITY = 20000;
-        //public static final int DRIVE_ACCELERATION = 7000;
+        public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
+        public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
+        public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
+        public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
 
         /* Trajectory Constants */
         public static final double MAX_SPEED_METERS_PER_SECOND = 3;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 09ee4a4..b82cda6 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -145,7 +145,7 @@ public class RobotContainer {
       
         // resets the yaw of the pigeon
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotDrive.runMotionMagicPID(12, 0), m_robotDrive));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 36));
       
         // turn 45 degrees
         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 259c571..061b6e1 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
@@ -18,6 +18,7 @@ public class DriveStraightToPositionMM extends CommandBase {
   double m_targetPosOut;
   double m_targetGyro;
   boolean isGoneFast;
+  int i;
   
   /**
    * Creates a new DriveToDistancePID.
@@ -39,6 +40,7 @@ public class DriveStraightToPositionMM extends CommandBase {
     m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
     m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
     isGoneFast = false;
+    i = 0;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -48,6 +50,8 @@ public class DriveStraightToPositionMM extends CommandBase {
     //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
     //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
     m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro);
+    SmartDashboard.putBoolean("MM Run", true);
+    i++;
   }
 
   // Called once the command ends or is interrupted.
@@ -59,9 +63,10 @@ public class DriveStraightToPositionMM extends CommandBase {
   @Override
   public boolean isFinished() {
     if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
+      SmartDashboard.putBoolean("MM Run", false);
       return true;
     } else {
-      if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) {
+      if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
         isGoneFast = true;
       }
       return false;
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 5387e8d..375ebd2 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -48,7 +48,7 @@ public class DriveWithJoystick extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    double cosMultiplier = .45;
+    double cosMultiplier = .55;
     double deadzone = .2;
     if (steerInput > 0){
       steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
index dbdd737..59add12 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
@@ -99,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   }
 
   private void runDriveWithInput(double move, double steerInput) {
-    double cosMultiplier = .45;
+    double cosMultiplier = .55;
     double steerOutput = 0;
     double deadzone = .2;
     /* Curves the steer output to be similarily gradual */
@@ -119,7 +119,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   }
 
   private void runStoppedTurn(double steer) {
-    double cosMultiplier = 0.70;
+    double cosMultiplier = 0.55;
     double steerOutput = 0;
     double deadzone = .2;
     /* Curves the steer output to be similarily gradual */
@@ -145,8 +145,8 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   private void updateGyroTarget(double steerInput) {
     m_targetGyro -= 5 * steerInput * m_deltaTime;
     m_targetGyro = MathUtil.clamp(  m_targetGyro,
-                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
-                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
+                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/2),
+                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/2));
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index b9a58b7..1faef12 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -74,7 +74,7 @@ public class Drive extends SubsystemBase {
   public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
   public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
   public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
-  //public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
+  public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
   
   public final DifferentialDriveOdometry m_odometry;
   
@@ -149,15 +149,15 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
 
-    //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
-    //m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
+    m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* PID for Back Motor control in Auto */
     m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
@@ -456,6 +456,7 @@ public class Drive extends SubsystemBase {
 
     m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
     m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
+
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
 

From 80dfa95f79824de4db5755a5441dd772025aa746 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 20:51:09 -0700
Subject: [PATCH 18/31] fox

---
 src/main/java/frc4388/robot/subsystems/Storage.java | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index aed7e4e..5a7c7a0 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -64,7 +64,6 @@ public class Storage extends SubsystemBase {
   
   public void runStorage(final double input) {
     m_storageMotor.set(input);
-    final boolean beam_on = m_beamSensors[0].get();
   }
 
   public void resetEncoder(){
@@ -74,12 +73,12 @@ public class Storage extends SubsystemBase {
   /* Storage PID Control */
   public void runStoragePositionPID(double targetPos){
     // Set PID Coefficients
-    m_storagePIDController.setP(m_storageGains.m_kP);
-    m_storagePIDController.setI(m_storageGains.m_kI);
-    m_storagePIDController.setD(m_storageGains.m_kD);
-    m_storagePIDController.setIZone(m_storageGains.m_kIzone);
-    m_storagePIDController.setFF(m_storageGains.m_kF);
-    m_storagePIDController.setOutputRange(StorageConstants.storkminOutput, m_storageGains.m_kmaxOutput);
+    m_storagePIDController.setP(storageGains.m_kP);
+    m_storagePIDController.setI(storageGains.m_kI);
+    m_storagePIDController.setD(storageGains.m_kD);
+    m_storagePIDController.setIZone(storageGains.m_kIzone);
+    m_storagePIDController.setFF(storageGains.m_kF);
+    m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
 
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }

From d8e35e77df459adb8aa1a71cc6a8dfb5e06303a8 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 21:11:43 -0700
Subject: [PATCH 19/31] Requested reviews

---
 src/main/java/frc4388/robot/subsystems/Intake.java  | 4 ++--
 src/main/java/frc4388/robot/subsystems/Storage.java | 3 +--
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index d6b9b32..cbcd617 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -55,7 +55,7 @@ public class Intake extends SubsystemBase {
    * Runs intake motor
    * @param input the percent output to run motor at
    */
-  public void runIntake(final double input) {
+  public void runIntake(double input) {
     m_intakeMotor.set(input);
   }
   
@@ -63,7 +63,7 @@ public class Intake extends SubsystemBase {
    * Runs extender motor
    * @param input the percent output to run motor at
    */
-  public void runExtender(final double input) {
+  public void runExtender() {
     if (m_extenderForwardLimit.get()) {
       isExtended = true;
     }
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 5a7c7a0..3d460fe 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -42,7 +42,6 @@ public class Storage extends SubsystemBase {
    */
   public Storage() {
     resetEncoder();
-    boolean botReached = false;
     m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0);
     m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1);
     m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2);
@@ -62,7 +61,7 @@ public class Storage extends SubsystemBase {
    * @param input the voltage to run motor at
    */
   
-  public void runStorage(final double input) {
+  public void runStorage(double input) {
     m_storageMotor.set(input);
   }
 

From ac1bb02a7a28cabd0b9aff57f18670aca91bb7a4 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 21:18:12 -0700
Subject: [PATCH 20/31] Fix input for extender

---
 src/main/java/frc4388/robot/subsystems/Intake.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index cbcd617..6b716f4 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -63,7 +63,7 @@ public class Intake extends SubsystemBase {
    * Runs extender motor
    * @param input the percent output to run motor at
    */
-  public void runExtender() {
+  public void runExtender(double input) {
     if (m_extenderForwardLimit.get()) {
       isExtended = true;
     }
@@ -72,10 +72,10 @@ public class Intake extends SubsystemBase {
     }
     
     if (isExtended == false) {
-      m_extenderMotor.set(0.5);
+      m_extenderMotor.set(input);
     }
     if (isExtended == true) {
-      m_extenderMotor.set(-0.5);
+      m_extenderMotor.set(-input);
     }
   }
 }
\ No newline at end of file

From 5015432107b3a6928d334ceb5f29213485db5dc8 Mon Sep 17 00:00:00 2001
From: Aarav Shah <91212@psdschools.org>
Date: Fri, 21 Feb 2020 22:03:41 -0700
Subject: [PATCH 21/31] Account for High gear being a nice boy

---
 src/main/java/frc4388/robot/Constants.java    |   9 ++
 .../DriveWithJoystickUsingDeadAssistPID.java  |  44 +++++--
 .../java/frc4388/robot/subsystems/Drive.java  | 114 ++++++++++++------
 3 files changed, 121 insertions(+), 46 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 1de9634..d146f9c 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -33,6 +33,15 @@ public final class Constants {
         public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
         public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
         public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final int DRIVE_CRUISE_VELOCITY = 20000;
+        public static final int DRIVE_ACCELERATION = 7000;
+
+        public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.05, 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 int DRIVE_CRUISE_VELOCITY_HIGH = 20000;
+        public static final int DRIVE_ACCELERATION_HIGH = 7000;
 
         /* Trajectory Constants */
         public static final double MAX_SPEED_METERS_PER_SECOND = 3;
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
index 59add12..6c60e40 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
@@ -24,6 +24,22 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   IHandController m_controller;
   boolean m_isInterrupted;
 
+  /* Deadassist Constants */
+  final float stopPosVelCoefLow = 1;
+  final float stopPosVelCoefHigh = 3;
+  final float cosMultiplierLow = 0.55f;
+  final float cosMultiplierHigh = 0.35f;
+  final float targetAngleCoefLow = 5;
+  final float targetAngleCoefHigh = 5;
+  final float gyroVelCoefLow = 1;
+  final float gyroVelCoefHigh = 3;
+
+  /* Deadassist Coeficients */
+  final float stopPosVelCoef = 1;
+  final float cosMultiplier = 0.55f;
+  final float targetAngleCoef = 5;
+  final float gyroVelCoef = 1;
+
   /**
    * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
    * Applies a curved ramp to the input from the controllers to make the robot less "touchy".
@@ -73,15 +89,19 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
       m_deadTimeSteer = m_currTime;
     }
 
-    /* If move stick has been pressed within 1 sec */
-    if (m_currTime - m_deadTimeMove < m_deadTimeout) {
-      /* Curves the moveInput to be slightly more gradual at first */
-      if (moveInput >= 0) {
-        moveOutput = -Math.cos(1.571*moveInput)+1;
-      } else {
-        moveOutput = Math.cos(1.571*moveInput)-1;
-      }
+    /* Curves the moveInput to be slightly more gradual at first */
+    if (moveInput >= 0) {
+      moveOutput = -Math.cos(1.571*moveInput)+1;
+    } else {
+      moveOutput = Math.cos(1.571*moveInput)-1;
+    }
 
+    if (m_drive.isSpeedShiftHigh) {
+      runDriveWithInput(moveOutput, steerInput);
+      resetGyroTarget();
+    }
+    /* If move stick has been pressed within 1 sec */
+    else if (m_currTime - m_deadTimeMove < m_deadTimeout) {
       /* If steer stick has not been used for less than 1 sec */
       if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
         runDriveWithInput(moveOutput, steerInput);
@@ -131,7 +151,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
 
     updateGyroTarget(steerOutput);
     double currentPos = m_drive.m_rightFrontMotorPos;
-    if (Math.abs(currentPos - m_stopPos) > 100) {
+    if (Math.abs(currentPos - m_stopPos) > 200) {
       m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
     } else {
       m_drive.driveWithInputAux(0, m_targetGyro);
@@ -145,15 +165,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   private void updateGyroTarget(double steerInput) {
     m_targetGyro -= 5 * steerInput * m_deltaTime;
     m_targetGyro = MathUtil.clamp(  m_targetGyro,
-                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/2),
-                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/2));
+                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/3),
+                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/3));
   }
 
   /**
    * set target angle to current angle (prevents buildup of gyro error).
    */
   private void resetGyroTarget() {
-    m_targetGyro = m_currentGyro;
+    //m_targetGyro = m_currentGyro;
     m_targetGyro =  m_currentGyro
                     + m_drive.getTurnRate();
   }
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 1faef12..d63e597 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -75,10 +75,17 @@ public class Drive extends SubsystemBase {
   public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
   public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
   public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
+
+  public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH;
+  public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH;
+  public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_HIGH;
+  public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH;
   
   public final DifferentialDriveOdometry m_odometry;
   
   public DoubleSolenoid m_speedShift;
+  public boolean isSpeedShiftHigh;
+
   public DoubleSolenoid m_coolFalcon;
 
   SendableChooser<String> m_songChooser = new SendableChooser<String>();
@@ -128,36 +135,7 @@ public class Drive extends SubsystemBase {
     //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
 
     /* PID for Front Motor Control in Teleop */
-    //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
-    //m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
-    m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
-    m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
-    m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    
 
     /* PID for Back Motor control in Auto */
     m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
@@ -371,6 +349,72 @@ public class Drive extends SubsystemBase {
                                               -inchesToMeters(getDistanceInches(m_rightBackMotor)));
   }
 
+  public void setRightMotorGains(boolean isHighGear) {
+    if (!isHighGear) {
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    } else {
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    }
+  }
+
   /**
    * Sets Motors to a NeutralMode.
    * 
@@ -705,11 +749,13 @@ public class Drive extends SubsystemBase {
    */
   public void setShiftState(boolean state) {
     if (state == true) {
-			m_speedShift.set(DoubleSolenoid.Value.kForward);
-		}
-		if (state == false) {
 			m_speedShift.set(DoubleSolenoid.Value.kReverse);
-		}
+    }
+		if (state == false) {
+			m_speedShift.set(DoubleSolenoid.Value.kForward);
+    }
+    setRightMotorGains(state);
+    isSpeedShiftHigh = state;
   }
 
   /**

From 56f4e09f8c134ae956172fcc201b4030923fc7b7 Mon Sep 17 00:00:00 2001
From: Sebastian <66741@psdschools.org>
Date: Sat, 22 Feb 2020 15:34:10 -0700
Subject: [PATCH 22/31] Added the CSV reading things

---
 src/main/deploy/Robot Data - Angles.csv       |   6 +
 src/main/deploy/Robot Data - Distances.csv    |   6 +
 .../java/frc4388/robot/RobotContainer.java    |   1 -
 .../frc4388/robot/subsystems/Shooter.java     |  15 ++
 .../java/frc4388/utility/ShooterTables.java   | 149 ++++++++++++++++++
 5 files changed, 176 insertions(+), 1 deletion(-)
 create mode 100644 src/main/deploy/Robot Data - Angles.csv
 create mode 100644 src/main/deploy/Robot Data - Distances.csv
 create mode 100644 src/main/java/frc4388/utility/ShooterTables.java

diff --git a/src/main/deploy/Robot Data - Angles.csv b/src/main/deploy/Robot Data - Angles.csv
new file mode 100644
index 0000000..273d4ba
--- /dev/null
+++ b/src/main/deploy/Robot Data - Angles.csv	
@@ -0,0 +1,6 @@
+Angle (deg),Displacement (deg)
+-20,-5
+-10,-2
+0,0
+10,2
+20,5
\ No newline at end of file
diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv
new file mode 100644
index 0000000..e058b92
--- /dev/null
+++ b/src/main/deploy/Robot Data - Distances.csv	
@@ -0,0 +1,6 @@
+Distance (in),Hood Ext. (u),Drum Velocity (u/ds)
+21,10,10000
+100,23,11000
+200,30,14000
+300,56,17000
+480,100,20000
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index b82cda6..c9a4ff8 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -13,7 +13,6 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.command.WaitCommand;
 import edu.wpi.first.wpilibj.controller.RamseteController;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index a18aca5..a62be93 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.ShooterConstants;
+import frc4388.utility.ShooterTables;
 import frc4388.utility.controller.IHandController;
 
 public class Shooter extends SubsystemBase {
@@ -48,6 +49,8 @@ public class Shooter extends SubsystemBase {
 
   double velP;
   double input;
+
+  ShooterTables m_shooterTable;
   
   /*
    * Creates a new Shooter subsystem.
@@ -71,6 +74,18 @@ public class Shooter extends SubsystemBase {
     int closedLoopTimeMs = 1;
     m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
     m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
+
+    m_shooterTable = new ShooterTables();
+
+    SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
+    SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
+    SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
+    SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500));
+
+    SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30));
+    SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
+    SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
+    SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
   }
 
   @Override
diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java
new file mode 100644
index 0000000..638f140
--- /dev/null
+++ b/src/main/java/frc4388/utility/ShooterTables.java
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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.utility;
+
+import java.io.BufferedReader;
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.FileReader;
+import java.io.IOException;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Add your docs here.
+ */
+public class ShooterTables {
+    double[][] m_distance = new double[50][3];
+    double[][] m_angle = new double[50][2];
+
+    final int m_columnDistance = 0;
+    final int m_columnHood = 1;
+    final int m_columnVel = 2;
+    final int m_columnAngle = 0;
+    final int m_columnDisplacement = 1;
+
+    int m_distanceLength;
+    int m_angleLength;
+
+    public ShooterTables() {
+        int lineNum = 0;
+
+        File distanceCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Distances.csv");
+        File angleCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Angles.csv");
+        
+        try {
+
+
+            BufferedReader distanceReader = new BufferedReader(new FileReader(distanceCSV));
+            BufferedReader angleReader = new BufferedReader(new FileReader(angleCSV));
+            String line = "";
+
+            while ((line = distanceReader.readLine()) != null) {
+
+                if (lineNum == 0) {
+                    lineNum ++;
+                } else {
+                    String[] values = line.split(",");
+
+                    m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]);
+                    m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]);
+                    m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]);
+                    
+                    lineNum ++;
+                }
+
+            }
+
+            m_distanceLength = lineNum-1;
+            lineNum = 0;
+
+            while ((line = angleReader.readLine()) != null) {
+
+                if (lineNum == 0) {
+                    lineNum ++;
+                } else {
+                    String[] values = line.split(",");
+
+                    m_angle[lineNum - 1][m_columnAngle] = Double.parseDouble(values[0]);
+                    m_angle[lineNum - 1][m_columnDisplacement] = Double.parseDouble(values[1]);
+                    
+                    lineNum ++;
+                }
+            }
+
+            m_angleLength = lineNum-1;
+
+        } catch (FileNotFoundException e) {
+            e.printStackTrace();
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+        SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]);
+        SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]);
+        SmartDashboard.putNumber("m_distanceLength", m_distanceLength);
+        SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]);
+        SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]);
+        SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]);
+    }
+
+    public double getHood(double distance) {
+        int i = 0;
+        while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
+            i ++;
+        }
+        if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
+            return m_distance[i][m_columnHood];
+        } else {
+            if (i >= m_distanceLength) {
+                i = m_distanceLength - 1;
+            }
+            return linearInterpolation(i, distance, m_columnHood, m_distance);
+        }
+    }
+
+    public double getVelocity(double distance) {
+        int i = 0;
+        while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
+            i ++;
+        }
+        if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
+            return m_distance[i][m_columnVel];
+        } else {
+            if (i >= m_distanceLength) {
+                i = m_distanceLength - 1;
+            }
+            return linearInterpolation(i, distance, m_columnVel, m_distance);
+        }
+    }
+
+    public double getAngleDisplacement(double angle) {
+        int i = 0;
+        while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) {
+            i ++;
+        }
+        if ((i < m_angleLength) && (m_angle[i][m_columnAngle] == angle)) {
+            return m_angle[i][m_columnDisplacement];
+        } else {
+            if (i >= m_angleLength) {
+                i = m_angleLength - 1;
+            }
+            return linearInterpolation(i, angle, m_columnDisplacement, m_angle);
+        }
+    }
+
+    public double linearInterpolation(int i, double value, int column, double[][] table) {
+        if (i != 0) {
+            double slope = (table[i][column] - table[i-1][column]) / (table[i][0] - table[i-1][0]);
+            value = slope * (value - table[i-1][0]) + table[i-1][column];
+            return value;
+        }
+        return 0.0;
+    }
+}
\ No newline at end of file

From ff274f96f41b88edb90dde01c3250f526aff3c64 Mon Sep 17 00:00:00 2001
From: nathanrsxtn <nathanrsxtn@gmail.com>
Date: Mon, 24 Feb 2020 19:48:38 -0700
Subject: [PATCH 23/31] Initial - Untested

---
 .../java/frc4388/robot/RobotContainer.java    |  4 +-
 .../robot/commands/DrivePositionMPAux.java    | 90 +++++++++++++++++++
 2 files changed, 93 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc4388/robot/commands/DrivePositionMPAux.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c9a4ff8..6d7765f 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
+import frc4388.robot.commands.DrivePositionMPAux;
 import frc4388.robot.commands.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.DriveStraightToPositionMM;
@@ -234,7 +235,8 @@ public class RobotContainer {
 
         // Run path following command, then stop at the end.
         return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/
-        return new InstantCommand();
+        // return new InstantCommand();
+        return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
     }
 
     /**
diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
new file mode 100644
index 0000000..364846a
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+
+public class DrivePositionMPAux extends CommandBase {
+  Drive m_drive;
+  double m_cruiseVel;
+  double m_rampDist;
+  double m_targetPos;
+  double m_currentVel;
+  double m_currentPos;
+  double m_targetGyro;
+  double m_targetVel;
+  double m_rampAcc;
+  long m_startTime;
+  long m_rampRate;
+
+  /**
+   * Creates a new DrivePositionMPAux.
+   * 
+   * @param subsystem The drive subsystem
+   * @param cruiseVel The target velocity for the motors in units
+   * @param rampDist  The distance before cruise velocity is reached in inches
+   * @param rampRate  The time to reach the cruise velocity in seconds
+   * @param targetPos The target position
+   */
+  public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH / 10;
+    m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH;
+    m_rampRate = (long) rampRate * 1000;
+    m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH;
+    //m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360;
+    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_currentVel = m_drive.m_rightFrontMotorVel;
+    m_currentPos = m_drive.m_rightFrontMotorPos;
+    m_targetPos = m_targetPos + m_currentPos;
+    m_targetVel = m_currentVel;
+    m_startTime = System.currentTimeMillis();
+    m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
+  }
+
+  // Called every m_isRamptime the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentVel = m_drive.m_rightFrontMotorVel;
+    m_currentPos = m_drive.m_rightFrontMotorPos;
+    if (System.currentTimeMillis() - m_startTime < m_rampRate) {
+      // Ramping
+      m_targetVel += m_rampAcc * m_drive.m_deltaTime;
+      m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+    } else if (m_targetPos - m_currentPos > m_rampDist) {
+      // Cruising
+      m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
+    } else {
+      // Deramp PID
+      m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH) {
+      return true;
+    }
+    return false;
+  }
+}

From ac82fdad12536a06ede77167ce39720cdf8afc41 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 25 Feb 2020 16:15:48 -0700
Subject: [PATCH 24/31] Fix Gearing Issues

---
 src/main/java/frc4388/robot/Constants.java    |  29 ++--
 .../commands/DriveStraightToPositionMM.java   |   2 +-
 .../commands/DriveStraightToPositionPID.java  |   2 +-
 .../DriveWithJoystickUsingDeadAssistPID.java  |   2 +-
 .../java/frc4388/robot/subsystems/Drive.java  | 132 ++++++++++--------
 5 files changed, 92 insertions(+), 75 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index d146f9c..b2d0534 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -29,14 +29,14 @@ public final class Constants {
 
         /* PID Constants Drive*/
         public static final int DRIVE_TIMEOUT_MS = 30;
-        public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
-        public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
-        public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
-        public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
+        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.05, 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 int DRIVE_CRUISE_VELOCITY = 20000;
         public static final int DRIVE_ACCELERATION = 7000;
 
-        public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.5);
+        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);
@@ -65,20 +65,27 @@ public final class Constants {
         
         /* Drive Train Characteristics */
         public static final double TICKS_PER_MOTOR_REV = 2048;
-        public static final double MOTOR_ROT_PER_WHEEL_ROT = 5.13;
+        public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
+        public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15;
         public static final double WHEEL_DIAMETER_INCHES = 6;
         public static final double TICKS_PER_GYRO_REV = 8192;
         
         /* Ratio Calculation */
+        public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
         public static final double TICK_TIME_TO_SECONDS = 0.1;
         public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
-        public static final double WHEEL_ROT_PER_MOTOR_ROT = 1/MOTOR_ROT_PER_WHEEL_ROT;
-        public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT;
-        public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
-        public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
-        public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
         public static final double INCHES_PER_METER = 39.370;
         public static final double METERS_PER_INCH = 1/INCHES_PER_METER;
+        
+        public static final double WHEEL_ROT_PER_MOTOR_ROT_HIGH = 1/MOTOR_ROT_PER_WHEEL_ROT_HIGH;
+        public static final double TICKS_PER_WHEEL_REV_HIGH = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_HIGH;
+        public static final double TICKS_PER_INCH_HIGH = TICKS_PER_WHEEL_REV_HIGH/INCHES_PER_WHEEL_REV;
+        public static final double INCHES_PER_TICK_HIGH = 1/TICKS_PER_INCH_HIGH;
+
+        public static final double WHEEL_ROT_PER_MOTOR_ROT_LOW = 1/MOTOR_ROT_PER_WHEEL_ROT_LOW;
+        public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW;
+        public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV;
+        public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW;
     }
     
     public static final class IntakeConstants {
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
index 061b6e1..5eba4a1 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
@@ -28,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase {
   public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
index 3b74edf..3b73c1b 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
@@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
index 6c60e40..12d605c 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
@@ -96,7 +96,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    if (m_drive.isSpeedShiftHigh) {
+    if (m_drive.m_isSpeedShiftHigh) {
       runDriveWithInput(moveOutput, steerInput);
       resetGyroTarget();
     }
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index d63e597..6ff68f3 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -71,20 +71,20 @@ public class Drive extends SubsystemBase {
   public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
 
   SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
-  public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
-  public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
-  public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
-  public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
+  public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
+  public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
+  public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_LOW;
+  public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_LOW;
 
-  public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH;
-  public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH;
-  public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_HIGH;
-  public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH;
+  public static Gains m_gainsDistanceHigh = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH;
+  public static Gains m_gainsVelocityHigh = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH;
+  public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH;
+  public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH;
   
   public final DifferentialDriveOdometry m_odometry;
   
   public DoubleSolenoid m_speedShift;
-  public boolean isSpeedShiftHigh;
+  public boolean m_isSpeedShiftHigh;
 
   public DoubleSolenoid m_coolFalcon;
 
@@ -139,18 +139,18 @@ public class Drive extends SubsystemBase {
 
     /* PID for Back Motor control in Auto */
     m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
-    m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
 
     m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
-    m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* Setup Sensors for WPI_TalonFXs */
     resetEncoders();
@@ -306,12 +306,14 @@ public class Drive extends SubsystemBase {
       SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
       SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
 
-      SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
-      SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
+      //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
+      //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
       //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
       //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
-      SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
-      SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_leftFrontMotor));
+      SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_rightFrontMotor));
 
       SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
       SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
@@ -345,43 +347,12 @@ public class Drive extends SubsystemBase {
     }
 
     m_odometry.update(Rotation2d.fromDegrees( getHeading()),
-                                              inchesToMeters(getDistanceInches(m_leftBackMotor)),
-                                              -inchesToMeters(getDistanceInches(m_rightBackMotor)));
+                                              getDistanceInches(m_leftBackMotor),
+                                              -getDistanceInches(m_rightBackMotor));
   }
 
   public void setRightMotorGains(boolean isHighGear) {
     if (!isHighGear) {
-      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
-      m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-
-      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
-      m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-
-      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
-      m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-
-      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
-      m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
-    } else {
       m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
       m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
       m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -409,6 +380,37 @@ public class Drive extends SubsystemBase {
       m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
       m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
       m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    } else {
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
+      m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+      m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
       m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
       m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
       m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -650,8 +652,8 @@ public class Drive extends SubsystemBase {
    * @return The current wheel speeds.
    */
   public DifferentialDriveWheelSpeeds getWheelSpeeds() {
-    return new DifferentialDriveWheelSpeeds(  inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)), 
-                                              -inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
+    return new DifferentialDriveWheelSpeeds(  getVelocityInchesPerSecond(m_leftBackMotor), 
+                                              -getVelocityInchesPerSecond(m_rightBackMotor));
   }
 
   /**
@@ -698,7 +700,11 @@ public class Drive extends SubsystemBase {
    * @return The converted value in inches
    */
   public double  ticksToInches(double ticks) {
-    return ticks * DriveConstants.INCHES_PER_TICK;
+    if (m_isSpeedShiftHigh) {
+      return ticks * DriveConstants.INCHES_PER_TICK_HIGH;
+    } else {
+      return ticks * DriveConstants.INCHES_PER_TICK_LOW;
+    }
   }
 
   /**
@@ -707,7 +713,11 @@ public class Drive extends SubsystemBase {
    * @return The converted value in ticks
    */
   public double inchesToTicks(double inches) {
-    return inches * DriveConstants.TICKS_PER_INCH;
+    if (m_isSpeedShiftHigh) {
+      return inches * DriveConstants.TICKS_PER_INCH_HIGH;
+    } else {
+      return inches * DriveConstants.TICKS_PER_INCH_LOW;
+    }
   }
 
   /**
@@ -755,7 +765,7 @@ public class Drive extends SubsystemBase {
 			m_speedShift.set(DoubleSolenoid.Value.kForward);
     }
     setRightMotorGains(state);
-    isSpeedShiftHigh = state;
+    m_isSpeedShiftHigh = state;
   }
 
   /**

From b741417e0b8f423d630b059201a1b5c543c519d1 Mon Sep 17 00:00:00 2001
From: Elijah Price <elijah@theprices.net>
Date: Tue, 25 Feb 2020 17:12:56 -0700
Subject: [PATCH 25/31] initial testing

---
 src/main/java/frc4388/robot/Robot.java        |  2 +-
 .../java/frc4388/robot/RobotContainer.java    |  7 +-
 .../robot/commands/DriveWithJoystick.java     |  6 +-
 .../java/frc4388/robot/subsystems/Drive.java  | 69 ++++++++++++-------
 4 files changed, 56 insertions(+), 28 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 5ada949..ba82993 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -106,7 +106,7 @@ public class Robot extends TimedRobot {
 
   @Override
   public void teleopInit() {
-    m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
+    m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
     m_robotContainer.setDriveGearState(true);
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
     
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index f14c5d9..f46ff66 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -117,11 +117,14 @@ public class RobotContainer {
         
         // sets solenoids into high gear
         new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
 
         // sets solenoids into low gear
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
+
+        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+            .whileHeld(new InstantCommand(() -> m_robotDrive.driveWithInput(0, 0), m_robotDrive));
 
 
         /* Operator Buttons */
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 5387e8d..42aedfc 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -48,12 +48,14 @@ public class DriveWithJoystick extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    double cosMultiplier = .45;
+    double cosMultiplier = .55;
     double deadzone = .2;
     if (steerInput > 0){
       steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
-    } else {
+    } else if (steerInput < 0) {
       steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
+    } else {
+      steerOutput = 0;
     }
 
     m_drive.driveWithInput(moveOutput, steerOutput);
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 1902e0d..d0e7601 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -26,6 +26,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
 import com.ctre.phoenix.motorcontrol.SensorTerm;
 import com.ctre.phoenix.motorcontrol.StatusFrame;
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.music.Orchestra;
@@ -33,6 +34,7 @@ import com.ctre.phoenix.sensors.PigeonIMU;
 import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
 
 import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -69,6 +71,7 @@ public class Drive extends SubsystemBase {
   public double m_rightFrontMotorVel;
 
   public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
+  //public RobotDrive m_driveTrain2 = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor)
 
   SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
   public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
@@ -109,21 +112,31 @@ public class Drive extends SubsystemBase {
     coolFalcon(false);
     
     /* set back motors as followers */
-    m_leftBackMotor.follow(m_leftFrontMotor);
-    m_rightBackMotor.follow(m_rightFrontMotor);
+    //m_leftBackMotor.follow(m_leftFrontMotor);
+    //m_rightBackMotor.follow(m_rightFrontMotor);
 
     /* flip input so forward becomes back, etc */
     m_leftFrontMotor.setInverted(false);
-    m_rightFrontMotor.setInverted(true);
+    m_rightFrontMotor.setInverted(false);
     //m_driveTrain.setRightSideInverted(false);
-    m_leftBackMotor.setInverted(InvertType.FollowMaster);
-    m_rightBackMotor.setInverted(InvertType.FollowMaster);
+    //m_leftBackMotor.setInverted(InvertType.FollowMaster);
+    //m_rightBackMotor.setInverted(InvertType.FollowMaster);
+
+    float rampRate = 0.1f;
+    m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 20, 25, 0.01);
+    m_rightFrontMotor.configSupplyCurrentLimit(c);
+    m_leftFrontMotor.configSupplyCurrentLimit(c);
 
     setDriveTrainNeutralMode(NeutralMode.Coast);
 
     /* deadbands */
-    m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
-    m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
+    //m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
+    //m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
     //m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
     //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
 
@@ -322,26 +335,36 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
     
     try {
-      SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
+      //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
       //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
       //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
+      SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
+      SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
       SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
       SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
-
-      SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
-      SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
+      //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
+      //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
       //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
       //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
-      SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
-      SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
-
+      /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity());
+      SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity());
+      */
       SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
       SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
 
-      //SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
-      //SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
-      //SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
-      //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent());
+
+
+      SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
+      SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
+      SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent());
+      SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent());
+      
+      SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
+      SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent());
+      SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent());
+      SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent());
 
       //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
       //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
@@ -350,10 +373,10 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
       //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
 
-      SmartDashboard.putString("Odometry Values Meters", getPose().toString());
-      SmartDashboard.putNumber("Odometry Heading", getHeading());
+      //SmartDashboard.putString("Odometry Values Meters", getPose().toString());
+      //SmartDashboard.putNumber("Odometry Heading", getHeading());
 
-      SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
+      //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
       //SmartDashboard.putNumber("Delta Time", m_deltaTime);
 
       if (currentSong != m_songChooser.getSelected()){
@@ -389,8 +412,8 @@ public class Drive extends SubsystemBase {
    */
   public void driveWithInput(double move, double steer) {
     m_driveTrain.arcadeDrive(move, steer);
-    m_leftBackMotor.follow(m_leftFrontMotor);
-    m_rightBackMotor.follow(m_rightFrontMotor);
+    m_leftBackMotor.follow(m_leftFrontMotor, FollowerType.PercentOutput);
+    m_rightBackMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput);
   }
 
   /**

From 0541a2463c4197fca8f707c3397e5fd6a9a3b9af Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 25 Feb 2020 17:47:47 -0700
Subject: [PATCH 26/31] Odometry fixes (but not really)

- Seems to only work on the left motor in low gear...no idea why
---
 src/main/java/frc4388/robot/Robot.java        |  2 +-
 .../java/frc4388/robot/RobotContainer.java    |  2 +-
 .../robot/commands/DriveWithJoystick.java     |  4 ++-
 .../java/frc4388/robot/subsystems/Drive.java  | 34 +++++++++++++------
 4 files changed, 29 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 5ada949..eca8337 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -79,7 +79,7 @@ public class Robot extends TimedRobot {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
-    m_robotContainer.setDriveGearState(true);
+    //m_robotContainer.setDriveGearState(true);
     m_robotContainer.resetOdometry();
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
 
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index b82cda6..f540f1d 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -92,7 +92,7 @@ public class RobotContainer {
 
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
-        m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController()));
+        m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
         // drives intake with input from triggers on the opperator controller
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
         // drives climber with input from triggers on the opperator controller
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 375ebd2..42aedfc 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -52,8 +52,10 @@ public class DriveWithJoystick extends CommandBase {
     double deadzone = .2;
     if (steerInput > 0){
       steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
-    } else {
+    } else if (steerInput < 0) {
       steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
+    } else {
+      steerOutput = 0;
     }
 
     m_drive.driveWithInput(moveOutput, steerOutput);
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 6ff68f3..edbd73a 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -95,6 +95,11 @@ public class Drive extends SubsystemBase {
 
   public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
 
+  public double m_totalLeftDistanceInches = 0;
+  public double m_totalRightDistanceInches = 0;
+  public double m_lastLeftPosTicks = 0;
+  public double m_lastRightPosTicks = 0;
+
   /**
    * Add your docs here.
    */
@@ -106,7 +111,7 @@ public class Drive extends SubsystemBase {
     m_rightBackMotor.configFactoryDefault();
     m_pigeon.configFactoryDefault();
     resetGyroYaw();
-
+    
     m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), 
                                                 new Pose2d(0, 0, new Rotation2d()) );
 
@@ -298,6 +303,13 @@ public class Drive extends SubsystemBase {
 
     m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
     m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
+
+    m_totalRightDistanceInches += ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastRightPosTicks);
+    m_totalLeftDistanceInches += ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastLeftPosTicks);
+
+    m_odometry.update(Rotation2d.fromDegrees( getHeading()),
+                                              getDistanceInches(m_rightFrontMotor),
+                                              -getDistanceInches(m_rightFrontMotor));
     
     try {
       SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
@@ -312,8 +324,8 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
       //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
       //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
-      SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_leftFrontMotor));
-      SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_rightFrontMotor));
+      SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor));
+      SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor));
 
       SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
       SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
@@ -346,9 +358,8 @@ public class Drive extends SubsystemBase {
       // e.printStackTrace(System.err);
     }
 
-    m_odometry.update(Rotation2d.fromDegrees( getHeading()),
-                                              getDistanceInches(m_leftBackMotor),
-                                              -getDistanceInches(m_rightBackMotor));
+    m_lastRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition();
+    m_lastLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition();
   }
 
   public void setRightMotorGains(boolean isHighGear) {
@@ -470,7 +481,7 @@ public class Drive extends SubsystemBase {
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
 
-    //m_driveTrain.feedWatchdog();
+    m_driveTrain.feedWatchdog();
   }
 
   /**
@@ -488,7 +499,7 @@ public class Drive extends SubsystemBase {
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
 
-    //m_driveTrain.feedWatchdog();
+    m_driveTrain.feedWatchdog();
   }
 
   /**
@@ -619,9 +630,9 @@ public class Drive extends SubsystemBase {
 //lol
 //sko
 //ridge
-  /**
 //brayden=bad coder
-  * Returns the heading of the robot
+  /**
+   * Returns the heading of the robot
    * @return The robot's heading in degrees, from -180 to 180
    */
   public double getHeading() {
@@ -664,6 +675,9 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
     m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_totalLeftDistanceInches = 0;
+    m_totalRightDistanceInches = 0;
   }
 
   /**

From 7339658077b70d486beb1edcb2d10d6197660de8 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 25 Feb 2020 19:15:15 -0700
Subject: [PATCH 27/31] Fix watchdog feeds and steer output curving

---
 .../java/frc4388/robot/commands/DriveWithJoystick.java |  4 ++--
 src/main/java/frc4388/robot/subsystems/Drive.java      | 10 +++++-----
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 42aedfc..fe1c8e5 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -51,9 +51,9 @@ public class DriveWithJoystick extends CommandBase {
     double cosMultiplier = .55;
     double deadzone = .2;
     if (steerInput > 0){
-      steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
+      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
     } else if (steerInput < 0) {
-      steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
     } else {
       steerOutput = 0;
     }
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index d0e7601..9f2639b 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -119,8 +119,8 @@ public class Drive extends SubsystemBase {
     m_leftFrontMotor.setInverted(false);
     m_rightFrontMotor.setInverted(false);
     //m_driveTrain.setRightSideInverted(false);
-    //m_leftBackMotor.setInverted(InvertType.FollowMaster);
-    //m_rightBackMotor.setInverted(InvertType.FollowMaster);
+    m_leftBackMotor.setInverted(InvertType.FollowMaster);
+    m_rightBackMotor.setInverted(InvertType.FollowMaster);
 
     float rampRate = 0.1f;
     m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -128,9 +128,9 @@ public class Drive extends SubsystemBase {
     m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
     m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
 
-    SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 20, 25, 0.01);
-    m_rightFrontMotor.configSupplyCurrentLimit(c);
-    m_leftFrontMotor.configSupplyCurrentLimit(c);
+    //SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01);
+    //m_rightFrontMotor.configSupplyCurrentLimit(c);
+    //m_leftFrontMotor.configSupplyCurrentLimit(c);
 
     setDriveTrainNeutralMode(NeutralMode.Coast);
 

From ab6cc48c461052da84743ffeefa1871695ee52bb Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 25 Feb 2020 20:45:29 -0700
Subject: [PATCH 28/31] Reduce deadband for greater range of motion

---
 src/main/java/frc4388/robot/commands/DriveWithJoystick.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index fe1c8e5..f51621a 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -49,7 +49,7 @@ public class DriveWithJoystick extends CommandBase {
     }
 
     double cosMultiplier = .55;
-    double deadzone = .2;
+    double deadzone = .1;
     if (steerInput > 0){
       steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
     } else if (steerInput < 0) {

From f7dc572a4463d88d58f47f5ebf5358dd83b87953 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 25 Feb 2020 20:52:20 -0700
Subject: [PATCH 29/31] Minor Cleanup

Removed set up of Neutral Mode in the constructor because Neutral Mode is controlled in Robot.java
---
 src/main/java/frc4388/robot/Robot.java            | 1 -
 src/main/java/frc4388/robot/subsystems/Drive.java | 2 --
 2 files changed, 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index ba82993..feab056 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -64,7 +64,6 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
-    //m_robotContainer.setDriveGearState(true);
   }
 
   @Override
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 9f2639b..74b9ef9 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -132,8 +132,6 @@ public class Drive extends SubsystemBase {
     //m_rightFrontMotor.configSupplyCurrentLimit(c);
     //m_leftFrontMotor.configSupplyCurrentLimit(c);
 
-    setDriveTrainNeutralMode(NeutralMode.Coast);
-
     /* deadbands */
     //m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
     //m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed

From 3e829b4db8e2b13ce0cd94bda172ed84b1b7829b Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 25 Feb 2020 21:12:02 -0700
Subject: [PATCH 30/31] Remove extraneous code

---
 src/main/java/frc4388/robot/RobotContainer.java   | 6 +-----
 src/main/java/frc4388/robot/subsystems/Drive.java | 9 ++++-----
 2 files changed, 5 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index f46ff66..f57b496 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -123,13 +123,9 @@ public class RobotContainer {
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
 
-        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new InstantCommand(() -> m_robotDrive.driveWithInput(0, 0), m_robotDrive));
-
-
         /* Operator Buttons */
       
-            //TODO: Shooter Buttons
+        //TODO: Shooter Buttons
         // shoots until released
         //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
         //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 74b9ef9..0dae7c8 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -71,7 +71,6 @@ public class Drive extends SubsystemBase {
   public double m_rightFrontMotorVel;
 
   public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
-  //public RobotDrive m_driveTrain2 = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor)
 
   SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
   public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
@@ -112,8 +111,8 @@ public class Drive extends SubsystemBase {
     coolFalcon(false);
     
     /* set back motors as followers */
-    //m_leftBackMotor.follow(m_leftFrontMotor);
-    //m_rightBackMotor.follow(m_rightFrontMotor);
+    m_leftBackMotor.follow(m_leftFrontMotor);
+    m_rightBackMotor.follow(m_rightFrontMotor);
 
     /* flip input so forward becomes back, etc */
     m_leftFrontMotor.setInverted(false);
@@ -410,8 +409,8 @@ public class Drive extends SubsystemBase {
    */
   public void driveWithInput(double move, double steer) {
     m_driveTrain.arcadeDrive(move, steer);
-    m_leftBackMotor.follow(m_leftFrontMotor, FollowerType.PercentOutput);
-    m_rightBackMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput);
+    m_leftBackMotor.follow(m_leftFrontMotor);
+    m_rightBackMotor.follow(m_rightFrontMotor);
   }
 
   /**

From 322e4c7a44e05d2f515f554ae8ac4947ef6a6b0f Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 25 Feb 2020 21:33:12 -0700
Subject: [PATCH 31/31] Change PIDs to expect robot to be in Low Gear

Eventually should be changed to be gear agnostic. Maybe have conversion factor be in drive subsytem and change depending on gear shift.
---
 .../java/frc4388/robot/commands/DrivePositionMPAux.java   | 8 ++++----
 .../frc4388/robot/commands/DriveStraightToPositionMM.java | 2 +-
 .../robot/commands/DriveStraightToPositionPID.java        | 2 +-
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
index 364846a..6883468 100644
--- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
@@ -36,10 +36,10 @@ public class DrivePositionMPAux extends CommandBase {
   public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH / 10;
-    m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH;
+    m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
+    m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH_LOW;
     m_rampRate = (long) rampRate * 1000;
-    m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH;
+    m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
     //m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360;
     m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
     addRequirements(m_drive);
@@ -82,7 +82,7 @@ public class DrivePositionMPAux extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH) {
+    if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) {
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
index 5eba4a1..c56a36b 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
@@ -28,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase {
   public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
index 3b73c1b..949a4bf 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
@@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }