From 653d9908ad024397fa849f76c72798029a2f5b1f Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 21:58:56 -0700
Subject: [PATCH 01/69] Split shooter.java

---
 .../java/frc4388/robot/RobotContainer.java    | 12 ++-
 .../robot/commands/StorageIntakeGroup.java    |  2 +-
 .../frc4388/robot/commands/TrackTarget.java   |  7 +-
 .../frc4388/robot/commands/storageOutake.java |  4 +-
 .../robot/commands/storagePrepAim.java        |  4 +-
 .../robot/commands/storagePrepIntake.java     |  4 +-
 .../frc4388/robot/subsystems/Shooter.java     | 74 +-------------
 .../frc4388/robot/subsystems/ShooterAim.java  | 98 +++++++++++++++++++
 8 files changed, 122 insertions(+), 83 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/subsystems/ShooterAim.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index f14c5d9..de8c407 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -44,13 +44,14 @@ import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
 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.commands.StorageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.Storage;
@@ -71,6 +72,7 @@ public class RobotContainer {
     private final LED m_robotLED = new LED();
     private final Intake m_robotIntake = new Intake();
     private final Shooter m_robotShooter = new Shooter();
+    private final ShooterAim m_robotShooterAim = new ShooterAim();
     private final Climber m_robotClimber = new Climber();
     private final Leveler m_robotLeveler = new Leveler();
     private final Storage m_robotStorage = new Storage();
@@ -99,7 +101,7 @@ 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));
+        m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.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()));
 
@@ -137,7 +139,7 @@ public class RobotContainer {
             
         // aims the turret
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter));
+            .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim));
             //.whenPressed(new RunCommand(() -> m_robotStorage.storeAim()));
         
         // extends or retracts the extender
@@ -151,7 +153,7 @@ public class RobotContainer {
 
         /* Storage Neo PID Test */
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter));
+            .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim));
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
@@ -159,7 +161,7 @@ public class RobotContainer {
             
         //Runs storage to outtake
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
-            .whileHeld(new storageOutake(m_robotStorage));
+            .whileHeld(new StorageOutake(m_robotStorage));
     }
        
     /**
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
index 767ed69..4b6cfee 100644
--- a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
+++ b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
@@ -20,7 +20,7 @@ public class StorageIntakeGroup extends SequentialCommandGroup {
    */
   public StorageIntakeGroup(Intake m_intake, Storage m_storage) {
     addCommands(
-      new storagePrepIntake(m_intake, m_storage), 
+      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/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 8102f13..374f04c 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -11,6 +11,7 @@ import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
 import frc4388.utility.controller.IHandController;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.networktables.NetworkTable;
@@ -21,6 +22,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 public class TrackTarget extends CommandBase {
     //Setup
   NetworkTableEntry xEntry;
+  ShooterAim m_shooterAim;
   Shooter m_shooter;
   IHandController m_driverController;
     //Aiming
@@ -35,7 +37,8 @@ public class TrackTarget extends CommandBase {
   /**
    * Uses the Limelight to track the target
    */
-  public TrackTarget(Shooter shooterSubsystem) {
+  public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
+    m_shooterAim = aimSubsystem;
     m_shooter = shooterSubsystem;
     addRequirements(m_shooter);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
@@ -65,7 +68,7 @@ public class TrackTarget extends CommandBase {
         //Deadzones
       else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
       else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
-      m_shooter.runShooterWithInput(turnAmount/5);
+      m_shooterAim.runShooterWithInput(turnAmount/5);
 
         //Finding Distance
       distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/storageOutake.java
index 4820dc0..48a28ba 100644
--- a/src/main/java/frc4388/robot/commands/storageOutake.java
+++ b/src/main/java/frc4388/robot/commands/storageOutake.java
@@ -10,12 +10,12 @@ package frc4388.robot.commands;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Storage;
 
-public class storageOutake extends CommandBase {
+public class StorageOutake extends CommandBase {
   Storage m_storage;
   /**
    * Creates a new storageOutake.
    */
-  public storageOutake(Storage storeSub) {
+  public StorageOutake(Storage storeSub) {
     m_storage = storeSub;
     addRequirements(m_storage);
   }
diff --git a/src/main/java/frc4388/robot/commands/storagePrepAim.java b/src/main/java/frc4388/robot/commands/storagePrepAim.java
index c8b3bad..1449ced 100644
--- a/src/main/java/frc4388/robot/commands/storagePrepAim.java
+++ b/src/main/java/frc4388/robot/commands/storagePrepAim.java
@@ -10,12 +10,12 @@ package frc4388.robot.commands;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Storage;
 
-public class storagePrepAim extends CommandBase {
+public class StoragePrepAim extends CommandBase {
   Storage m_storage;
   /**
    * Creates a new storagePrepAim.
    */
-  public storagePrepAim(Storage storeSub) {
+  public StoragePrepAim(Storage storeSub) {
     m_storage = storeSub;
     addRequirements(m_storage);
   }
diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
index 7fab016..3c0d1b5 100644
--- a/src/main/java/frc4388/robot/commands/storagePrepIntake.java
+++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
@@ -11,13 +11,13 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.Storage;
 
-public class storagePrepIntake extends CommandBase {
+public class StoragePrepIntake extends CommandBase {
   public Intake m_intake;
   public Storage m_storage;
   /**
    * Creates a new storagePrepIntake.
    */
-  public storagePrepIntake(Intake inSub, Storage storeSub) {
+  public StoragePrepIntake(Intake inSub, Storage storeSub) {
     m_intake = inSub;
     m_storage = storeSub;
     addRequirements(m_intake);
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index c4487ce..37be2a5 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -29,23 +29,12 @@ import frc4388.utility.controller.IHandController;
 public class Shooter extends SubsystemBase {
 
   public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
-  public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
-  public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
 
 
-  public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
   public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
   public static Shooter m_shooter;
   public static IHandController m_controller;
 
-
-  // Configure PID Controllers
-  CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
-  CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
-
-  CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
-  CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
-
   double velP;
   double input;
   public boolean velReached;
@@ -57,11 +46,8 @@ public class Shooter extends SubsystemBase {
    */
   public Shooter() {
     //Testing purposes reseting gyros
-    resetGyroAngleAdj();
-    resetGyroShooterRotate();
 
     m_shooterFalcon.configFactoryDefault();
-    m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
     m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
     m_shooterFalcon.setInverted(false);
     
@@ -102,11 +88,12 @@ public class Shooter extends SubsystemBase {
    */
   public void setShooterGains() {
     m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
-    m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
   }
+
   /**
    * Runs drum shooter velocity PID.
    * @param falcon Motor to use
@@ -136,55 +123,4 @@ public class Shooter extends SubsystemBase {
       velReached = false;
     }
   }
-
-
-  public void runShooterWithInput(double input) {
-    m_shooterRotateMotor.set(input);
-  }
-
-  /* Angle Adjustment PID Control */
-  public void runAngleAdjustPID(double targetAngle)
-  {
-    // Set PID Coefficients
-    m_angleAdjustPIDController.setP(m_shooterTurretGains.m_kP);
-    m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI);
-    m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD);
-    m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone);
-    m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF);
-    m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
-
-    // Convert input angle in degrees to rotations of the motor
-    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
-
-    m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
-  }
-
-  /* Rotate Shooter PID Control */
-  public void runshooterRotatePID(double targetAngle)
-  {
-    // Set PID Coefficients
-    m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
-    m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
-    m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
-    m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
-    m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
-    m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
-
-    // Convert input angle in degrees to rotations of the motor
-    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
-
-    m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
-  }
-
-  /* For Testing Purposes, reseting gyro for angle adjuster */
-  public void resetGyroAngleAdj()
-  {
-    m_angleEncoder.setPosition(0);
-  }
-
-  /* For Testing Purposes, reseting gyro for shooter rotation */
-  public void resetGyroShooterRotate()
-  {
-    m_shooterRotateEncoder.setPosition(0);
-  }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
new file mode 100644
index 0000000..9de24bd
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems;
+
+import com.revrobotics.CANEncoder;
+import com.revrobotics.CANPIDController;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.ControlType;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Gains;
+import frc4388.robot.Constants.ShooterConstants;
+
+public class ShooterAim extends SubsystemBase {
+  public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+  public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
+
+  public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
+
+    // Configure PID Controllers
+    CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
+    CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
+  
+    CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
+    CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
+  /**
+   * Creates a new ShooterAim.
+   */
+  public ShooterAim() {
+    resetGyroAngleAdj();
+    resetGyroShooterRotate();
+    m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
+    m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
+  }
+
+  public void runShooterWithInput(double input) {
+    m_shooterRotateMotor.set(input);
+  }
+
+  /* Angle Adjustment PID Control */
+  public void runAngleAdjustPID(double targetAngle)
+  {
+    // Set PID Coefficients
+    m_angleAdjustPIDController.setP(m_shooterTurretGains.m_kP);
+    m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI);
+    m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD);
+    m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone);
+    m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF);
+    m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
+
+    // Convert input angle in degrees to rotations of the motor
+    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
+
+    m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
+  }
+
+  /* Rotate Shooter PID Control */
+  public void runshooterRotatePID(double targetAngle)
+  {
+    // Set PID Coefficients
+    m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
+    m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
+    m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
+    m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
+    m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
+    m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
+
+    // Convert input angle in degrees to rotations of the motor
+    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
+
+    m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
+  }
+
+
+    /* For Testing Purposes, reseting gyro for angle adjuster */
+    public void resetGyroAngleAdj()
+    {
+      m_angleEncoder.setPosition(0);
+    }
+  
+    /* For Testing Purposes, reseting gyro for shooter rotation */
+    public void resetGyroShooterRotate()
+    {
+      m_shooterRotateEncoder.setPosition(0);
+    }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

From ab2db7d33c08855b4e6418e761ea35bfc5bb3a6c Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 21 Feb 2020 22:04:00 -0700
Subject: [PATCH 02/69] fix hoepfully

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

diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
index 3c0d1b5..202335e 100644
--- a/src/main/java/frc4388/robot/commands/storagePrepIntake.java
+++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
@@ -14,6 +14,7 @@ import frc4388.robot.subsystems.Storage;
 public class StoragePrepIntake extends CommandBase {
   public Intake m_intake;
   public Storage m_storage;
+
   /**
    * Creates a new storagePrepIntake.
    */

From 9b0907c0afa4a417a9b03768ed6f4a323a502542 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 09:58:37 -0700
Subject: [PATCH 03/69] ajhhhhhhhhhh

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

diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
index 202335e..9b5b89e 100644
--- a/src/main/java/frc4388/robot/commands/storagePrepIntake.java
+++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
@@ -16,7 +16,7 @@ public class StoragePrepIntake extends CommandBase {
   public Storage m_storage;
 
   /**
-   * Creates a new storagePrepIntake.
+   * Creates a new StoragePrepIntake.
    */
   public StoragePrepIntake(Intake inSub, Storage storeSub) {
     m_intake = inSub;

From 97d64f36ce2422c7aac47f2a2156b497ffcca378 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 10:37:56 -0700
Subject: [PATCH 04/69] CONSTATS

---
 src/main/java/frc4388/robot/Constants.java      |  7 +++++--
 .../robot/commands/StorageIntakeFinal.java      |  3 ++-
 .../robot/commands/StorageIntakeGroup.java      |  5 +++--
 .../frc4388/robot/commands/storageIntake.java   | 17 +++++++++++------
 .../frc4388/robot/commands/storageOutake.java   |  3 ++-
 .../frc4388/robot/commands/storagePrepAim.java  |  3 ++-
 .../robot/commands/storagePrepIntake.java       |  3 ++-
 7 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 0a5502a..8852e44 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -77,6 +77,7 @@ public final class Constants {
     public static final class IntakeConstants {
         public static final int INTAKE_SPARK_ID = -9;
         public static final int EXTENDER_SPARK_ID = -10;
+        public static final double EXTENDER_SPEED = 0.3;
     }
   
     public static final class ShooterConstants {
@@ -107,7 +108,10 @@ public final class Constants {
   
     public static final class StorageConstants {
         public static final int STORAGE_CAN_ID = -1;
-
+        public static final double STORAGE_PARTIAL_BALL = 2;
+        public static final double STORAGE_FULL_BALL = 7;
+        public static final double STORAGE_SPEED = 0.5;
+       
         /* Ball Indexes */
         public static final int BEAM_SENSOR_DIO_0 = 0;
         public static final int BEAM_SENSOR_DIO_1 = 1;
@@ -123,7 +127,6 @@ public final class Constants {
         public static final int PID_PRIMARY = 0;
 
         /* PID Gains */
-
         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);
     }
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
index 9576f35..7ac5f46 100644
--- a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
+++ b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
@@ -8,6 +8,7 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
 public class StorageIntakeFinal extends CommandBase {
@@ -29,7 +30,7 @@ public class StorageIntakeFinal extends CommandBase {
   @Override
   public void execute() {
     if (m_storage.getBeam(1)){
-      m_storage.setStoragePID(m_storage.getEncoderPos() + 5);
+      m_storage.setStoragePID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL);
     }
   }
 
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
index 4b6cfee..4636edc 100644
--- a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
+++ b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
@@ -21,7 +21,8 @@ public class StorageIntakeGroup extends SequentialCommandGroup {
   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));
+      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
index ee30708..30d73ce 100644
--- a/src/main/java/frc4388/robot/commands/storageIntake.java
+++ b/src/main/java/frc4388/robot/commands/storageIntake.java
@@ -8,16 +8,19 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.IntakeConstants;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.Storage;
 
-public class storageIntake extends CommandBase {
+public class StorageIntake extends CommandBase {
   public Intake m_intake;
   public Storage m_storage;
+  public boolean intook;
   /**
    * Creates a new storageIntake.
    */
-  public storageIntake(Intake inSub, Storage storeSub) {
+  public StorageIntake(Intake inSub, Storage storeSub) {
     m_intake = inSub;
     m_storage = storeSub;
     addRequirements(m_intake);
@@ -27,6 +30,7 @@ public class storageIntake extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    intook = false;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -34,11 +38,12 @@ public class storageIntake extends CommandBase {
   public void execute() {
 
     if (m_storage.getBeam(0)){
-      m_storage.setStoragePID(m_storage.getEncoderPos() + 2);
-      m_intake.runExtender(-0.3); 
+      m_storage.setStoragePID(m_storage.getEncoderPos() + StorageConstants.STORAGE_PARTIAL_BALL);
+      m_intake.runExtender(-IntakeConstants.EXTENDER_SPEED); 
+      intook = true;
     }
     else{
-      m_intake.runExtender(0.3); 
+      m_intake.runExtender(IntakeConstants.EXTENDER_SPEED); 
     }
   }
 
@@ -50,7 +55,7 @@ public class storageIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(0)){
+    if (!m_storage.getBeam(0) && m_storage.getBeam(1) && intook){
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/storageOutake.java
index 48a28ba..c19ce5a 100644
--- a/src/main/java/frc4388/robot/commands/storageOutake.java
+++ b/src/main/java/frc4388/robot/commands/storageOutake.java
@@ -8,6 +8,7 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
 public class StorageOutake extends CommandBase {
@@ -28,7 +29,7 @@ public class StorageOutake extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-      m_storage.runStorage(-0.5);
+      m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc4388/robot/commands/storagePrepAim.java b/src/main/java/frc4388/robot/commands/storagePrepAim.java
index 1449ced..376a92b 100644
--- a/src/main/java/frc4388/robot/commands/storagePrepAim.java
+++ b/src/main/java/frc4388/robot/commands/storagePrepAim.java
@@ -8,6 +8,7 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
 public class StoragePrepAim extends CommandBase {
@@ -29,7 +30,7 @@ public class StoragePrepAim extends CommandBase {
   @Override
   public void execute() {
     if (m_storage.getBeam(2) == false){
-      m_storage.runStorage(0.5);
+      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
     }
     else{
       m_storage.runStorage(0);
diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
index 9b5b89e..3321027 100644
--- a/src/main/java/frc4388/robot/commands/storagePrepIntake.java
+++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java
@@ -8,6 +8,7 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.Storage;
 
@@ -34,7 +35,7 @@ public class StoragePrepIntake extends CommandBase {
   @Override
   public void execute() {
     if (m_storage.getBeam(1) == false){
-      m_storage.runStorage(-0.5);
+      m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
     }
     else{
       m_storage.runStorage(0);

From 58c9c1d4304d0939ba979f82e6c160a2b267d612 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 10:48:14 -0700
Subject: [PATCH 05/69] Rename storageIntake.java to StorageIntake.java

---
 .../robot/commands/{storageIntake.java => StorageIntake.java}     | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename src/main/java/frc4388/robot/commands/{storageIntake.java => StorageIntake.java} (100%)

diff --git a/src/main/java/frc4388/robot/commands/storageIntake.java b/src/main/java/frc4388/robot/commands/StorageIntake.java
similarity index 100%
rename from src/main/java/frc4388/robot/commands/storageIntake.java
rename to src/main/java/frc4388/robot/commands/StorageIntake.java

From 15dbcff010831f52894b49ed6a5ad68da4337781 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 10:48:44 -0700
Subject: [PATCH 06/69] Rename storageOutake.java to StorageOutake.java

---
 .../robot/commands/{storageOutake.java => StorageOutake.java}     | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename src/main/java/frc4388/robot/commands/{storageOutake.java => StorageOutake.java} (100%)

diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/StorageOutake.java
similarity index 100%
rename from src/main/java/frc4388/robot/commands/storageOutake.java
rename to src/main/java/frc4388/robot/commands/StorageOutake.java

From a8bced06d1996605a7b4dbd9da4a5b8db6ebf3e1 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 10:49:00 -0700
Subject: [PATCH 07/69] Rename storagePrepAim.java to StoragePrepAim.java

---
 .../robot/commands/{storagePrepAim.java => StoragePrepAim.java}   | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename src/main/java/frc4388/robot/commands/{storagePrepAim.java => StoragePrepAim.java} (100%)

diff --git a/src/main/java/frc4388/robot/commands/storagePrepAim.java b/src/main/java/frc4388/robot/commands/StoragePrepAim.java
similarity index 100%
rename from src/main/java/frc4388/robot/commands/storagePrepAim.java
rename to src/main/java/frc4388/robot/commands/StoragePrepAim.java

From ae450e76f012a5cf705ed51e7f0a07663ddca1eb Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 10:49:16 -0700
Subject: [PATCH 08/69] Rename storagePrepIntake.java to StoragePrepIntake.java

---
 .../commands/{storagePrepIntake.java => StoragePrepIntake.java}   | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename src/main/java/frc4388/robot/commands/{storagePrepIntake.java => StoragePrepIntake.java} (100%)

diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
similarity index 100%
rename from src/main/java/frc4388/robot/commands/storagePrepIntake.java
rename to src/main/java/frc4388/robot/commands/StoragePrepIntake.java

From 0b77117f50bfa47e69b0e83a65c6b6b2dd3515aa Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 11:23:57 -0700
Subject: [PATCH 09/69] Shooter Groups Framework

---
 src/main/java/frc4388/robot/Constants.java    |  3 +-
 .../robot/commands/ShootFireGroup.java        | 24 ++++++++++++++
 .../robot/commands/ShootFullGroup.java        | 25 +++++++++++++++
 .../robot/commands/ShootPrepGroup.java        | 23 +++++++++++++
 .../frc4388/robot/subsystems/Shooter.java     | 32 +++++++++++++++++--
 .../frc4388/robot/subsystems/ShooterAim.java  | 30 -----------------
 6 files changed, 103 insertions(+), 34 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/ShootFireGroup.java
 create mode 100644 src/main/java/frc4388/robot/commands/ShootFullGroup.java
 create mode 100644 src/main/java/frc4388/robot/commands/ShootPrepGroup.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 8852e44..3a44301 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -91,7 +91,8 @@ public final class Constants {
         public static final int SHOOTER_PID_LOOP_IDX = 1;
         public static final int SHOOTER_TIMEOUT_MS = 30;
         public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
-        public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
         public static final double SHOOTER_TURRET_MIN = -1.0;
         public static final double ENCODER_TICKS_PER_REV = 2048;
         public static final double NEO_UNITS_PER_REV = 42;
diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
new file mode 100644
index 0000000..155e384
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* 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.ParallelRaceGroup;
+
+// 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 ShootFireGroup extends ParallelRaceGroup {
+  /**
+   * Creates a new ShootFireGroup.
+   */
+  public ShootFireGroup() {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    super();
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
new file mode 100644
index 0000000..1bb7956
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+// 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 ShootFullGroup extends SequentialCommandGroup {
+  /**
+   * Creates a new ShootFullGroup.
+   */
+  public ShootFullGroup() {
+    super(
+      new ShootPrepGroup(), 
+      new ShootFireGroup()
+    );
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
new file mode 100644
index 0000000..58ab9f1
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* 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.ParallelCommandGroup;
+
+// 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 ShootPrepGroup extends ParallelCommandGroup {
+  /**
+   * Creates a new ShootPrepGroup.
+   */
+  public ShootPrepGroup() {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());super();
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index 37be2a5..ecd2a88 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -29,12 +29,16 @@ import frc4388.utility.controller.IHandController;
 public class Shooter extends SubsystemBase {
 
   public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
+  public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
 
+  CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
+  CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
 
   public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
+  public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
   public static Shooter m_shooter;
   public static IHandController m_controller;
-
+  
   double velP;
   double input;
   public boolean velReached;
@@ -46,11 +50,11 @@ public class Shooter extends SubsystemBase {
    */
   public Shooter() {
     //Testing purposes reseting gyros
-
+    resetGyroAngleAdj();
     m_shooterFalcon.configFactoryDefault();
     m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
     m_shooterFalcon.setInverted(false);
-    
+    m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
     setShooterGains();
 
     m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -94,6 +98,23 @@ public class Shooter extends SubsystemBase {
     m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
   }
 
+  /* Angle Adjustment PID Control */
+  public void runAngleAdjustPID(double targetAngle)
+  {
+    // Set PID Coefficients
+    m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
+    m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
+    m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD);
+    m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone);
+    m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
+    m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); 
+
+    // Convert input angle in degrees to rotations of the motor
+    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
+
+    m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
+  }
+
   /**
    * Runs drum shooter velocity PID.
    * @param falcon Motor to use
@@ -123,4 +144,9 @@ public class Shooter extends SubsystemBase {
       velReached = false;
     }
   }
+
+  public void resetGyroAngleAdj()
+    {
+      m_angleEncoder.setPosition(0);
+    }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
index 9de24bd..1640937 100644
--- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -13,53 +13,29 @@ import com.revrobotics.CANSparkMax;
 import com.revrobotics.ControlType;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.ShooterConstants;
 
 public class ShooterAim extends SubsystemBase {
-  public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
   public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
-
   public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
 
     // Configure PID Controllers
-    CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
     CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
-  
-    CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
     CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
   /**
    * Creates a new ShooterAim.
    */
   public ShooterAim() {
-    resetGyroAngleAdj();
     resetGyroShooterRotate();
     m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
-    m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
   }
 
   public void runShooterWithInput(double input) {
     m_shooterRotateMotor.set(input);
   }
 
-  /* Angle Adjustment PID Control */
-  public void runAngleAdjustPID(double targetAngle)
-  {
-    // Set PID Coefficients
-    m_angleAdjustPIDController.setP(m_shooterTurretGains.m_kP);
-    m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI);
-    m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD);
-    m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone);
-    m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF);
-    m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
-
-    // Convert input angle in degrees to rotations of the motor
-    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
-
-    m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
-  }
 
   /* Rotate Shooter PID Control */
   public void runshooterRotatePID(double targetAngle)
@@ -78,12 +54,6 @@ public class ShooterAim extends SubsystemBase {
     m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
   }
 
-
-    /* For Testing Purposes, reseting gyro for angle adjuster */
-    public void resetGyroAngleAdj()
-    {
-      m_angleEncoder.setPosition(0);
-    }
   
     /* For Testing Purposes, reseting gyro for shooter rotation */
     public void resetGyroShooterRotate()

From 5534ff6ef289cf78086910a858c2e076c2c1a2cb Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 11:28:41 -0700
Subject: [PATCH 10/69] CApitals

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index de8c407..fbd7c2a 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -52,6 +52,7 @@ import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.commands.StorageOutake;
+import frc4388.robot.commands.StoragePrepAim;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.Storage;
@@ -139,8 +140,8 @@ public class RobotContainer {
             
         // aims the turret
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim));
-            //.whenPressed(new RunCommand(() -> m_robotStorage.storeAim()));
+            .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim))
+            .whenPressed(new StoragePrepAim(m_robotStorage));
         
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)

From 6c8a1dffb37ce51e62ee3e26a77e44de3607066f Mon Sep 17 00:00:00 2001
From: Rishabh <57200@psdschools.org>
Date: Sat, 22 Feb 2020 12:06:14 -0700
Subject: [PATCH 11/69] test

---
 .../robot/commands/ShootFireGroup.java        | 14 +++++-
 .../robot/commands/StoragePositionPID.java    | 46 +++++++++++++++++++
 2 files changed, 58 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/StoragePositionPID.java

diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
index 155e384..cd78283 100644
--- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -8,6 +8,10 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.Storage;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -16,9 +20,15 @@ public class ShootFireGroup extends ParallelRaceGroup {
   /**
    * Creates a new ShootFireGroup.
    */
-  public ShootFireGroup() {
+  public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
+
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
-    super();
+    super(
+      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
+      new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
+      new TrackTarget(m_shooter, m_shooterAim),
+      new 
+      );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
new file mode 100644
index 0000000..083aea6
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/StoragePositionPID.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.Constants.StorageConstants;
+import frc4388.robot.Subsystems.Storage;
+
+
+public class StoragePositionPID extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new StoragePositionPID.
+   */
+  public StoragePositionPID(Storage storage) {
+    m_storage = storage;
+    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() {
+    new runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL)
+  }
+
+  // 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;
+  }
+}

From 741b1299087ff67a328f7fc56d6601ebaf15d8ac Mon Sep 17 00:00:00 2001
From: Rishabh <57200@psdschools.org>
Date: Sat, 22 Feb 2020 12:11:50 -0700
Subject: [PATCH 12/69] Started fire sturf

---
 src/main/java/frc4388/robot/commands/ShootFireGroup.java     | 3 +--
 src/main/java/frc4388/robot/commands/StoragePositionPID.java | 5 +++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
index cd78283..d0e0ab6 100644
--- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -27,8 +27,7 @@ public class ShootFireGroup extends ParallelRaceGroup {
     super(
       new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
       new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
-      new TrackTarget(m_shooter, m_shooterAim),
-      new 
+      new TrackTarget(m_shooter, m_shooterAim)
       );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
index 083aea6..1270d3c 100644
--- a/src/main/java/frc4388/robot/commands/StoragePositionPID.java
+++ b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
@@ -8,8 +8,9 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.robot.Constants.StorageConstants;
-import frc4388.robot.Subsystems.Storage;
+import frc4388.robot.subsystems.Storage;
 
 
 public class StoragePositionPID extends CommandBase {
@@ -30,7 +31,7 @@ public class StoragePositionPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    new runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL)
+    new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL));
   }
 
   // Called once the command ends or is interrupted.

From 77e536d2b15d5144e796378f60aefaca118895c5 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 13:01:50 -0700
Subject: [PATCH 13/69] Shooter PREP

---
 .../frc4388/robot/commands/HoodAdjustPID.java | 44 +++++++++++++++++++
 .../robot/commands/ShootFullGroup.java        |  7 ++-
 .../robot/commands/ShootPrepGroup.java        | 16 +++++--
 .../commands/ShooterVelocityControlPID.java   | 13 +++++-
 .../frc4388/robot/commands/TrackTarget.java   |  9 +++-
 .../frc4388/robot/subsystems/Shooter.java     | 11 -----
 6 files changed, 80 insertions(+), 20 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/HoodAdjustPID.java

diff --git a/src/main/java/frc4388/robot/commands/HoodAdjustPID.java b/src/main/java/frc4388/robot/commands/HoodAdjustPID.java
new file mode 100644
index 0000000..2815dd5
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/HoodAdjustPID.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.Shooter;
+
+public class HoodAdjustPID extends CommandBase {
+  Shooter m_shooter;
+  /**
+   * Creates a new HoodAdjustPID.
+   */
+  public HoodAdjustPID(Shooter shooterSub) {
+    m_shooter = shooterSub;
+    addRequirements(m_shooter);
+  }
+
+  // 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_shooter.runAngleAdjustPID(m_shooter.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() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
index 1bb7956..9da770d 100644
--- a/src/main/java/frc4388/robot/commands/ShootFullGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
@@ -8,6 +8,9 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.Storage;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -16,9 +19,9 @@ public class ShootFullGroup extends SequentialCommandGroup {
   /**
    * Creates a new ShootFullGroup.
    */
-  public ShootFullGroup() {
+  public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     super(
-      new ShootPrepGroup(), 
+      new ShootPrepGroup(m_shooter, m_shooterAim, m_storage), 
       new ShootFireGroup()
     );
   }
diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
index 58ab9f1..1b17fda 100644
--- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
@@ -8,16 +8,24 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+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 ShootPrepGroup extends ParallelCommandGroup {
   /**
-   * Creates a new ShootPrepGroup.
+   * Preps the Shooter to be fired
    */
-  public ShootPrepGroup() {
-    // Add your commands in the super() call, e.g.
-    // super(new FooCommand(), new BarCommand());super();
+  public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
+    super(
+      new TrackTarget(m_shooter, m_shooterAim),
+      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
+      new HoodAdjustPID(m_shooter),
+      new StoragePrepAim(m_storage)
+      );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index 67b5cb1..f4aeae8 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -13,6 +13,7 @@ import frc4388.robot.subsystems.Shooter;
 public class ShooterVelocityControlPID extends CommandBase {
   Shooter m_shooter;
   double m_targetVel;
+  double m_actualVel;
   /**
    * Creates a new ShooterVelocityControlPID.
    */
@@ -31,8 +32,8 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
     m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorVelocity();
   }
 
   // Called once the command ends or is interrupted.
@@ -43,6 +44,14 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    //Tells wether the target velocity has been reached
+    double upperBound = m_targetVel + 300;
+    double lowerBound = m_targetVel - 300;
+    if (m_actualVel < upperBound && m_actualVel > lowerBound){
+      return true;
+    }
+    else{
+      return false;
+    }
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 374f04c..e892956 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -68,7 +68,7 @@ public class TrackTarget extends CommandBase {
         //Deadzones
       else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
       else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
-      m_shooterAim.runShooterWithInput(turnAmount/5);
+      m_shooterAim.runShooterWithInput(turnAmount/3);
 
         //Finding Distance
       distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
@@ -81,6 +81,7 @@ public class TrackTarget extends CommandBase {
       fireAngle = Math.atan(yVel/xVel);
       m_shooter.m_fireVel = fireVel;
       m_shooter.m_fireAngle = fireAngle;
+
     }
   }
 
@@ -95,6 +96,12 @@ public class TrackTarget extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
+    double upperLimit = xAngle + 0.05;
+    double lowerLimit = xAngle - 0.05;
+    if (xAngle < upperLimit && xAngle > lowerLimit)
+    {
+      return true;
+    }
     return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index ecd2a88..14d4080 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -132,17 +132,6 @@ public class Shooter extends SubsystemBase {
     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;
-    }
   }
 
   public void resetGyroAngleAdj()

From 2b4324effd546db5412f0b9f8980c0168c14a94c Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 13:16:27 -0700
Subject: [PATCH 14/69] Shooter firing

---
 .../frc4388/robot/commands/HoodAdjustPID.java |  3 +-
 .../robot/commands/ShootFireGroup.java        | 13 +++---
 .../robot/commands/ShootFullGroup.java        |  7 ++-
 .../robot/commands/ShootPrepGroup.java        |  3 ++
 .../commands/ShooterVelocityControlPID.java   |  2 +
 .../frc4388/robot/commands/StorageRun.java    | 45 +++++++++++++++++++
 6 files changed, 64 insertions(+), 9 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/StorageRun.java

diff --git a/src/main/java/frc4388/robot/commands/HoodAdjustPID.java b/src/main/java/frc4388/robot/commands/HoodAdjustPID.java
index 2815dd5..f08db0d 100644
--- a/src/main/java/frc4388/robot/commands/HoodAdjustPID.java
+++ b/src/main/java/frc4388/robot/commands/HoodAdjustPID.java
@@ -13,7 +13,8 @@ import frc4388.robot.subsystems.Shooter;
 public class HoodAdjustPID extends CommandBase {
   Shooter m_shooter;
   /**
-   * Creates a new HoodAdjustPID.
+   * Adjusts the hood based on the limelight target angle
+   * @param shooterSub The Shooter subsystem
    */
   public HoodAdjustPID(Shooter shooterSub) {
     m_shooter = shooterSub;
diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
index d0e0ab6..03371d7 100644
--- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -18,16 +18,17 @@ import frc4388.robot.subsystems.Storage;
 // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
 public class ShootFireGroup extends ParallelRaceGroup {
   /**
-   * Creates a new ShootFireGroup.
+   * Fires the shooter
+   * @param m_shooter The Shooter subsytem
+   * @param m_shooterAim The ShooterAim subsystem
+   * @param m_storage The Storage subsytem
    */
   public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
-
-    // Add your commands in the super() call, e.g.
-    // super(new FooCommand(), new BarCommand());
     super(
       new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
-      new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
-      new TrackTarget(m_shooter, m_shooterAim)
+      new HoodAdjustPID(m_shooter),
+      new TrackTarget(m_shooter, m_shooterAim),
+      new StorageRun(m_storage)
       );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
index 9da770d..63bc7ec 100644
--- a/src/main/java/frc4388/robot/commands/ShootFullGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
@@ -17,12 +17,15 @@ import frc4388.robot.subsystems.Storage;
 // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
 public class ShootFullGroup extends SequentialCommandGroup {
   /**
-   * Creates a new ShootFullGroup.
+   * Preps and Fires the Shooter
+   * @param m_shooter The Shooter subsytem
+   * @param m_shooterAim The ShooterAim subsystem
+   * @param m_storage The Storage subsytem
    */
   public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     super(
       new ShootPrepGroup(m_shooter, m_shooterAim, m_storage), 
-      new ShootFireGroup()
+      new ShootFireGroup(m_shooter, m_shooterAim, m_storage)
     );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
index 1b17fda..ffbc2b5 100644
--- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
@@ -19,6 +19,9 @@ import frc4388.robot.subsystems.Storage;
 public class ShootPrepGroup extends ParallelCommandGroup {
   /**
    * Preps the Shooter to be fired
+   * @param m_shooter The Shooter subsytem
+   * @param m_shooterAim The ShooterAim subsystem
+   * @param m_storage The Storage subsytem
    */
   public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     super(
diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index f4aeae8..4da0fe3 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -16,6 +16,8 @@ public class ShooterVelocityControlPID extends CommandBase {
   double m_actualVel;
   /**
    * Creates a new ShooterVelocityControlPID.
+   * @param subsystem The Shooter subsytem
+   * @param targetVel The target velocity
    */
   public ShooterVelocityControlPID(Shooter subsystem, double targetVel) {
     // Use addRequirements() here to declare subsystem dependencies.
diff --git a/src/main/java/frc4388/robot/commands/StorageRun.java b/src/main/java/frc4388/robot/commands/StorageRun.java
new file mode 100644
index 0000000..5b56b52
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/StorageRun.java
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* 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.StorageConstants;
+import frc4388.robot.subsystems.Storage;
+
+public class StorageRun extends CommandBase {
+  Storage m_storage;
+  /**
+   * Runs the Storage at a speed
+   * @param storageSub The Storage subsytem
+   */
+  public StorageRun(Storage storageSub) {
+    m_storage = storageSub;
+  }
+
+  // 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(StorageConstants.STORAGE_SPEED);
+  }
+
+  // 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;
+  }
+}

From 4888a4c08d5f69fcd0706eac73e20be9d3ec803e Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 13:37:38 -0700
Subject: [PATCH 15/69] Interuptability Handling

---
 .../java/frc4388/robot/RobotContainer.java    |  36 +++----
 .../frc4388/robot/commands/HoldTarget.java    | 101 ++++++++++++++++++
 .../robot/commands/ShootFireGroup.java        |   6 +-
 .../frc4388/robot/subsystems/Shooter.java     |   5 +-
 4 files changed, 124 insertions(+), 24 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/HoldTarget.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index fbd7c2a..01c4be8 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -47,6 +47,7 @@ import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
+import frc4388.robot.commands.ShootFullGroup;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
@@ -79,15 +80,15 @@ public class RobotContainer {
     private final Storage m_robotStorage = new Storage();
 
     /* Cameras */
-    private final Camera m_robotCameraFront = new Camera("front",0,160,120,40);
-    private final Camera m_robotCameraBack = new Camera("back",1,160,120,40);
+    private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
+    private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
 
     /* Controllers */
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
 
     /**
-     * The container for the robot.  Contains subsystems, OI devices, and commands.
+     * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
         configureButtonBindings();
@@ -102,22 +103,22 @@ 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_robotShooterAim.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
+        m_robotShooter.setDefaultCommand(new RunCommand(
+                () -> m_robotShooterAim.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()));
 
     }
-      
 
     /**
-    * Use this method to define your button->command mappings.  Buttons can be created by
-    * instantiating a {@link GenericHID} or one of its subclasses ({@link
-    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-    */
+     * Use this method to define your button->command mappings. Buttons can be
+     * created by instantiating a {@link GenericHID} or one of its subclasses
+     * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
     private void configureButtonBindings() {
         /* Driver Buttons */
-        
+
         // sets solenoids into high gear
         new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
@@ -126,17 +127,16 @@ public class RobotContainer {
         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));
+        new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
+            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
 
         // shoots one ball
-        //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-        //    .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1));
+        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
+            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
             
         // aims the turret
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java
new file mode 100644
index 0000000..8449078
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/HoldTarget.java
@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* 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 frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.utility.controller.IHandController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class HoldTarget extends CommandBase {
+    //Setup
+  NetworkTableEntry xEntry;
+  ShooterAim m_shooterAim;
+  Shooter m_shooter;
+  IHandController m_driverController;
+    //Aiming
+  double turnAmount = 0;
+  double xAngle = 0;
+  double yAngle = 0;
+  double target = 0;
+  public double distance;
+  public static double fireVel;
+  public static double fireAngle;
+
+  /**
+   * Uses the Limelight to track the target
+   */
+  public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
+    m_shooterAim = aimSubsystem;
+    m_shooter = shooterSubsystem;
+    addRequirements(m_shooter);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+      //Vision Processing Mode
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+  }
+
+
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
+    xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
+    yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
+    
+    if (target == 1.0){ //If target in view
+        //Aiming Left/Right
+      turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE;
+      if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone 
+        //Deadzones
+      else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
+      else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
+      m_shooterAim.runShooterWithInput(turnAmount/3);
+
+        //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);
+      m_shooter.m_fireVel = fireVel;
+      m_shooter.m_fireAngle = fireAngle;
+
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      //Drive Camera Mode
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
index 03371d7..0175198 100644
--- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -25,9 +25,9 @@ public class ShootFireGroup extends ParallelRaceGroup {
    */
   public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     super(
-      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
-      new HoodAdjustPID(m_shooter),
-      new TrackTarget(m_shooter, m_shooterAim),
+      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
+      new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
+      new HoldTarget(m_shooter, m_shooterAim),
       new StorageRun(m_storage)
       );
   }
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index 14d4080..5f9507c 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -134,8 +134,7 @@ public class Shooter extends SubsystemBase {
     }
   }
 
-  public void resetGyroAngleAdj()
-    {
+  public void resetGyroAngleAdj(){
       m_angleEncoder.setPosition(0);
-    }
+  }
 }
\ No newline at end of file

From 881c3d90762411194480682ee8e1e34a28046b4c Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 22 Feb 2020 15:03:10 -0700
Subject: [PATCH 16/69] Makework

---
 src/main/java/frc4388/robot/Constants.java    |  6 +--
 .../java/frc4388/robot/RobotContainer.java    | 13 +++---
 .../frc4388/robot/commands/HoodAdjustPID.java | 45 -------------------
 .../robot/commands/ShootFireGroup.java        | 10 ++---
 .../robot/commands/ShootFullGroup.java        |  2 +-
 .../robot/commands/ShootPrepGroup.java        |  9 ++--
 .../commands/ShooterVelocityControlPID.java   |  1 +
 .../frc4388/robot/commands/TrackTarget.java   |  6 +--
 .../frc4388/robot/subsystems/Shooter.java     |  4 +-
 9 files changed, 23 insertions(+), 73 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/commands/HoodAdjustPID.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 3a44301..481a7f5 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -82,9 +82,9 @@ public final class Constants {
   
     public static final class ShooterConstants {
         /* Motor IDs */
-        public static final int SHOOTER_FALCON_ID = -1;
-        public static final int SHOOTER_ANGLE_ADJUST_ID = -1;
-        public static final int SHOOTER_ROTATE_ID = 10;
+        public static final int SHOOTER_FALCON_ID = 8;
+        public static final int SHOOTER_ANGLE_ADJUST_ID = 10;
+        public static final int SHOOTER_ROTATE_ID = 9;
 
         /* PID Constants Shooter */
         public static final int SHOOTER_SLOT_IDX = 0;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 01c4be8..8e0dad0 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -34,6 +34,7 @@ import frc4388.robot.commands.DriveStraightToPositionMM;
 import frc4388.robot.commands.DriveStraightToPositionPID;
 import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
+import frc4388.robot.commands.HoldTarget;
 import frc4388.robot.commands.DriveWithJoystickDriveStraight;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
@@ -127,7 +128,7 @@ public class RobotContainer {
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
 
-            
+
         /* Operator Buttons */
 
         // shoots until released
@@ -137,11 +138,6 @@ public class RobotContainer {
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
-            
-        // aims the turret
-        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim))
-            .whenPressed(new StoragePrepAim(m_robotStorage));
         
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -152,9 +148,10 @@ public class RobotContainer {
             .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
             .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
 
-        /* Storage Neo PID Test */
+        // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim));
+            .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim));
+            //.whenPressed(new StoragePrepAim(m_robotStorage));
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
diff --git a/src/main/java/frc4388/robot/commands/HoodAdjustPID.java b/src/main/java/frc4388/robot/commands/HoodAdjustPID.java
deleted file mode 100644
index f08db0d..0000000
--- a/src/main/java/frc4388/robot/commands/HoodAdjustPID.java
+++ /dev/null
@@ -1,45 +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.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Shooter;
-
-public class HoodAdjustPID extends CommandBase {
-  Shooter m_shooter;
-  /**
-   * Adjusts the hood based on the limelight target angle
-   * @param shooterSub The Shooter subsystem
-   */
-  public HoodAdjustPID(Shooter shooterSub) {
-    m_shooter = shooterSub;
-    addRequirements(m_shooter);
-  }
-
-  // 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_shooter.runAngleAdjustPID(m_shooter.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() {
-    return false;
-  }
-}
diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
index 0175198..060334d 100644
--- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -24,11 +24,11 @@ public class ShootFireGroup extends ParallelRaceGroup {
    * @param m_storage The Storage subsytem
    */
   public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
-    super(
+    addCommands(
       new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
       new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
-      new HoldTarget(m_shooter, m_shooterAim),
-      new StorageRun(m_storage)
-      );
+      new HoldTarget(m_shooter, m_shooterAim)
+      //new StorageRun(m_storage)
+    );
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
index 63bc7ec..55557b8 100644
--- a/src/main/java/frc4388/robot/commands/ShootFullGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFullGroup.java
@@ -23,7 +23,7 @@ public class ShootFullGroup extends SequentialCommandGroup {
    * @param m_storage The Storage subsytem
    */
   public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
-    super(
+    addCommands(
       new ShootPrepGroup(m_shooter, m_shooterAim, m_storage), 
       new ShootFireGroup(m_shooter, m_shooterAim, m_storage)
     );
diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
index ffbc2b5..169640e 100644
--- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
@@ -24,11 +24,10 @@ public class ShootPrepGroup extends ParallelCommandGroup {
    * @param m_storage The Storage subsytem
    */
   public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
-    super(
+    addCommands(
       new TrackTarget(m_shooter, m_shooterAim),
-      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
-      new HoodAdjustPID(m_shooter),
-      new StoragePrepAim(m_storage)
-      );
+      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel())
+      //new StoragePrepAim(m_storage)
+    );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index 4da0fe3..1cac750 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -35,6 +35,7 @@ public class ShooterVelocityControlPID extends CommandBase {
   @Override
   public void execute() {
     m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
     m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorVelocity();
   }
 
diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index e892956..db5e3a1 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -40,7 +40,7 @@ public class TrackTarget extends CommandBase {
   public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
     m_shooterAim = aimSubsystem;
     m_shooter = shooterSubsystem;
-    addRequirements(m_shooter);
+    addRequirements(m_shooterAim);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
   }
 
@@ -96,9 +96,7 @@ public class TrackTarget extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    double upperLimit = xAngle + 0.05;
-    double lowerLimit = xAngle - 0.05;
-    if (xAngle < upperLimit && xAngle > lowerLimit)
+    if (xAngle < 1 && xAngle > -1)
     {
       return true;
     }
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index 5f9507c..69d0721 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -127,10 +127,10 @@ public class Shooter extends SubsystemBase {
     runSpeed = runSpeed/targetVel; //Convert to percent
 
     if (actualVel < targetVel - 1000){ //PID Based on ramp up amount
-      m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed);
+      m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/3);
     }
     else{ //PID Based on targetVel
-      m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
+      m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel/3); //Init PID
     }
   }
 

From 371cc7ba4a5c0a648e81b1218bd1c1390abf481a Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sun, 23 Feb 2020 21:22:52 -0700
Subject: [PATCH 17/69] yuh

---
 .../java/frc4388/robot/commands/ShooterVelocityControlPID.java   | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index 1cac750..ad295e3 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -20,7 +20,6 @@ public class ShooterVelocityControlPID extends CommandBase {
    * @param targetVel The target velocity
    */
   public ShooterVelocityControlPID(Shooter subsystem, double targetVel) {
-    // Use addRequirements() here to declare subsystem dependencies.
     m_shooter = subsystem;
     m_targetVel = targetVel;
     addRequirements(m_shooter);

From 716dc834809d4a8073d0b49556a9b5cc285096e6 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Tue, 25 Feb 2020 17:44:59 -0700
Subject: [PATCH 18/69] Fixing the thing

---
 src/main/java/frc4388/robot/Constants.java    |  3 ++-
 .../java/frc4388/robot/RobotContainer.java    | 19 +++++++++++++++----
 .../robot/commands/ShootFireGroup.java        |  2 +-
 .../robot/commands/ShootPrepGroup.java        |  2 +-
 .../commands/ShooterVelocityControlPID.java   |  7 +++----
 .../frc4388/robot/subsystems/Shooter.java     | 13 ++++++-------
 6 files changed, 28 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 481a7f5..f81591d 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -90,7 +90,8 @@ public final class Constants {
         public static final int SHOOTER_SLOT_IDX = 0;
         public static final int SHOOTER_PID_LOOP_IDX = 1;
         public static final int SHOOTER_TIMEOUT_MS = 30;
-        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
+        //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
+        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0, 0.0453, 0, 1.0);
         public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
         public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
         public static final double SHOOTER_TURRET_MIN = -1.0;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 8e0dad0..5339635 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -48,7 +48,9 @@ import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
+import frc4388.robot.commands.ShootFireGroup;
 import frc4388.robot.commands.ShootFullGroup;
+import frc4388.robot.commands.ShootPrepGroup;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
@@ -104,10 +106,11 @@ 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_robotShooterAim.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
+        m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
         // drives the leveler with an axis input from the driver controller
         m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
+        // moves the drum not
+        m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
 
     }
 
@@ -137,8 +140,10 @@ public class RobotContainer {
 
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
-        
+            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity())));
+            //.whenReleased(new InstantCommand(() -> m_robotShooterAim.runShooterWithInput(0), m_robotShooterAim));
+             
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
             .whenPressed(new RunExtenderOutIn(m_robotIntake));
@@ -160,6 +165,12 @@ public class RobotContainer {
         //Runs storage to outtake
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
             .whileHeld(new StorageOutake(m_robotStorage));
+
+        new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
+            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage));
+
+        new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
+            .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity())));
     }
        
     /**
diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
index 060334d..855fc48 100644
--- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -25,7 +25,7 @@ public class ShootFireGroup extends ParallelRaceGroup {
    */
   public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     addCommands(
-      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
+      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(7000, m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
       new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
       new HoldTarget(m_shooter, m_shooterAim)
       //new StorageRun(m_storage)
diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
index 169640e..5d60abb 100644
--- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
@@ -26,7 +26,7 @@ public class ShootPrepGroup extends ParallelCommandGroup {
   public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     addCommands(
       new TrackTarget(m_shooter, m_shooterAim),
-      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel())
+      new ShooterVelocityControlPID(m_shooter)
       //new StoragePrepAim(m_storage)
     );
   }
diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index ad295e3..a2a1b77 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -19,23 +19,22 @@ public class ShooterVelocityControlPID extends CommandBase {
    * @param subsystem The Shooter subsytem
    * @param targetVel The target velocity
    */
-  public ShooterVelocityControlPID(Shooter subsystem, double targetVel) {
+  public ShooterVelocityControlPID(Shooter subsystem) {
     m_shooter = subsystem;
-    m_targetVel = targetVel;
     addRequirements(m_shooter);
   }
 
   // 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_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_shooter.runDrumShooterVelocityPID(7000, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
     m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
-    m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorVelocity();
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index 69d0721..3de7f36 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -55,6 +55,7 @@ public class Shooter extends SubsystemBase {
     m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
     m_shooterFalcon.setInverted(false);
     m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
+    m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
     setShooterGains();
 
     m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
@@ -101,6 +102,7 @@ public class Shooter extends SubsystemBase {
   /* Angle Adjustment PID Control */
   public void runAngleAdjustPID(double targetAngle)
   {
+    targetAngle = addFireAngle();
     // Set PID Coefficients
     m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
     m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
@@ -121,17 +123,14 @@ public class Shooter extends SubsystemBase {
    * @param targetVel Target velocity to run motor at
    */
   public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
-    velP = actualVel/targetVel; //Percent of target
-    double runSpeed = actualVel + (12000*velP); //Ramp up equation
     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/3);
+    if (actualVel < targetVel - 1000){
+      m_shooterFalcon.set(TalonFXControlMode.PercentOutput, 1);//Ramp up
     }
     else{ //PID Based on targetVel
-      m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel/3); //Init PID
+      m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
     }
+    m_shooterFalcon.feed();
   }
 
   public void resetGyroAngleAdj(){

From 1bad4f73f4f3723678c6f09088c3eb149563a6b8 Mon Sep 17 00:00:00 2001
From: mayabartels <mayabartels01@gmail.com>
Date: Tue, 25 Feb 2020 19:32:43 -0700
Subject: [PATCH 19/69] Testing and fixing the intake

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

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 0a5502a..6d194b5 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -75,8 +75,8 @@ public final class Constants {
     }
     
     public static final class IntakeConstants {
-        public static final int INTAKE_SPARK_ID = -9;
-        public static final int EXTENDER_SPARK_ID = -10;
+        public static final int INTAKE_SPARK_ID = 12;
+        public static final int EXTENDER_SPARK_ID = 13;
     }
   
     public static final class ShooterConstants {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index f14c5d9..006f0e0 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -154,8 +154,8 @@ public class RobotContainer {
             .whileHeld(new TrackTarget(m_robotShooter));
 
         //Prepares storage for intaking
-        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
-            .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage));
+        new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
+            .whileHeld(new InstantCommand(() -> m_robotIntake.runIntake(0.3), m_robotIntake));
             
         //Runs storage to outtake
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)

From 4cd54197469b822ed8e06a6eae1d31b89e010f36 Mon Sep 17 00:00:00 2001
From: mayabartels <mayabartels01@gmail.com>
Date: Tue, 25 Feb 2020 19:40:05 -0700
Subject: [PATCH 20/69] reversed triggers to fix intake motor direction

---
 .../frc4388/robot/commands/RunIntakeWithTriggers.java  | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java
index bb7cef2..5f9dc4a 100644
--- a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java
+++ b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java
@@ -41,15 +41,15 @@ public class RunIntakeWithTriggers extends CommandBase {
     double rightTrigger = m_controller.getRightTriggerAxis();
     double leftTrigger = m_controller.getLeftTriggerAxis();
     double output = 0;
-    if (rightTrigger < .5) {
-      if(rightTrigger > leftTrigger) {
-        output = rightTrigger;
+    if (leftTrigger < .5) {
+      if(leftTrigger > rightTrigger) {
+        output = leftTrigger;
       }
-      if (leftTrigger > rightTrigger) {
+      if (rightTrigger > leftTrigger) {
         output = -leftTrigger;
       }
     } else {
-      output = rightTrigger;
+      output = leftTrigger;
     }
     m_intake.runIntake(output);
   }

From 011146b9ec24f8e468f6ce3df60fdaacf600d678 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Tue, 25 Feb 2020 20:01:24 -0700
Subject: [PATCH 21/69] Ugly but functional

---
 src/main/java/frc4388/robot/Constants.java    |  2 +-
 .../java/frc4388/robot/RobotContainer.java    | 13 +++--
 .../frc4388/robot/commands/HoldTarget.java    |  7 ++-
 .../frc4388/robot/commands/TrackTarget.java   | 54 ++++++++++---------
 .../java/frc4388/robot/subsystems/Camera.java |  4 +-
 .../frc4388/robot/subsystems/LimeLight.java   | 35 ++++++++++++
 .../frc4388/robot/subsystems/Shooter.java     |  4 +-
 .../frc4388/robot/subsystems/ShooterAim.java  |  2 +
 .../frc4388/robot/subsystems/Storage.java     | 11 ++++
 9 files changed, 94 insertions(+), 38 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/subsystems/LimeLight.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index f81591d..8f67b12 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -93,7 +93,7 @@ public final class Constants {
         //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
         public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0, 0.0453, 0, 1.0);
         public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
-        public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
         public static final double SHOOTER_TURRET_MIN = -1.0;
         public static final double ENCODER_TICKS_PER_REV = 2048;
         public static final double NEO_UNITS_PER_REV = 42;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 5339635..b65ee53 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -59,6 +59,7 @@ import frc4388.robot.commands.StorageOutake;
 import frc4388.robot.commands.StoragePrepAim;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
+import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.subsystems.Storage;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
@@ -85,6 +86,7 @@ public class RobotContainer {
     /* Cameras */
     private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
     private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
+    private final LimeLight m_robotLime = new LimeLight();
 
     /* Controllers */
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
@@ -111,6 +113,8 @@ public class RobotContainer {
         m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
         // moves the drum not
         m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
+        //turns limelight off
+        //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
 
     }
 
@@ -140,10 +144,11 @@ public class RobotContainer {
 
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
-            .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity())));
-            //.whenReleased(new InstantCommand(() -> m_robotShooterAim.runShooterWithInput(0), m_robotShooterAim));
-             
+            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
+            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+            //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity())))
+            //.whenReleased(new RunCommand(() -> m_robotShooterAim.limeOff()))
+            //.whenReleased(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(0)));
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
             .whenPressed(new RunExtenderOutIn(m_robotIntake));
diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java
index 8449078..cbe43cf 100644
--- a/src/main/java/frc4388/robot/commands/HoldTarget.java
+++ b/src/main/java/frc4388/robot/commands/HoldTarget.java
@@ -10,6 +10,7 @@ package frc4388.robot.commands;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.utility.controller.IHandController;
@@ -48,8 +49,7 @@ public class HoldTarget extends CommandBase {
   @Override
   public void initialize() {
       //Vision Processing Mode
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+    LimeLight.limeOn();
   }
 
 
@@ -89,8 +89,7 @@ public class HoldTarget extends CommandBase {
   @Override
   public void end(boolean interrupted) {
       //Drive Camera Mode
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+    LimeLight.limeOff();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index db5e3a1..647341b 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -9,23 +9,26 @@ package frc4388.robot.commands;
 
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.utility.controller.IHandController;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class TrackTarget extends CommandBase {
-    //Setup
+  // Setup
   NetworkTableEntry xEntry;
   ShooterAim m_shooterAim;
   Shooter m_shooter;
   IHandController m_driverController;
-    //Aiming
+  // Aiming
   double turnAmount = 0;
   double xAngle = 0;
   double yAngle = 0;
@@ -47,38 +50,40 @@ public class TrackTarget extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-      //Vision Processing Mode
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+    // Vision Processing Mode
+    LimeLight.limeOn();
   }
 
-
-
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
     xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
     yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
-    
-    if (target == 1.0){ //If target in view
-        //Aiming Left/Right
-      turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE;
-      if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone 
-        //Deadzones
-      else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
-      else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
-      m_shooterAim.runShooterWithInput(turnAmount/3);
 
-        //Finding Distance
-      distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
+    if (target == 1.0) { // If target in view
+      // Aiming Left/Right
+      turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE;
+      if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
+        turnAmount = 0;
+      } // Angle Error Zone
+      // Deadzones
+      else if (turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE) {
+        turnAmount = VisionConstants.MOTOR_DEAD_ZONE;
+      } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
+        turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
+      }
+      m_shooterAim.runShooterWithInput(turnAmount / 3);
+
+      // 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);
+      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);
+      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;
 
@@ -88,9 +93,8 @@ public class TrackTarget extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-      //Drive Camera Mode
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+    // Drive Camera Mode
+    LimeLight.limeOff();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc4388/robot/subsystems/Camera.java b/src/main/java/frc4388/robot/subsystems/Camera.java
index e483009..332bb46 100644
--- a/src/main/java/frc4388/robot/subsystems/Camera.java
+++ b/src/main/java/frc4388/robot/subsystems/Camera.java
@@ -11,6 +11,7 @@ import edu.wpi.cscore.MjpegServer;
 import edu.wpi.cscore.UsbCamera;
 import edu.wpi.cscore.VideoSource;
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 
@@ -38,8 +39,7 @@ public class Camera extends SubsystemBase {
     catch(Exception e){
       System.err.println("Camera broken, pls nerf");
     }
-
-  }
+  }  
 
   @Override
   public void periodic() {
diff --git a/src/main/java/frc4388/robot/subsystems/LimeLight.java b/src/main/java/frc4388/robot/subsystems/LimeLight.java
new file mode 100644
index 0000000..70c53ae
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/LimeLight.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class LimeLight extends SubsystemBase {
+  /**
+   * Creates a new LimeLight.
+   */
+  public LimeLight() {
+
+  }
+
+  public static void limeOff(){
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+  }
+  
+  public static void limeOn(){
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index 3de7f36..8648ff9 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -100,9 +100,9 @@ public class Shooter extends SubsystemBase {
   }
 
   /* Angle Adjustment PID Control */
-  public void runAngleAdjustPID(double targetAngle)
+  public void runAngleAdjustPID(double mmtargetAngle)
   {
-    targetAngle = addFireAngle();
+    double targetAngle = addFireAngle();
     // Set PID Coefficients
     m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
     m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
index 1640937..b6ffef2 100644
--- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -13,6 +13,8 @@ import com.revrobotics.CANSparkMax;
 import com.revrobotics.ControlType;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.ShooterConstants;
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 3d460fe..5dbcc44 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -94,4 +94,15 @@ public class Storage extends SubsystemBase {
   public void setStoragePID(double position){
     m_storagePIDController.setReference(position , ControlType.kPosition);
   }
+
+
+  /*
+  If pressing aim
+    Run until hitting bottom beam
+    dont run intake if balls not at bottom
+  else
+    run unti; hitting top beam
+  
+  2 beamms total
+  */
 }

From 553aa6ad942cbca1036d5e347a09db144b1eeadc Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Wed, 26 Feb 2020 08:25:39 -0700
Subject: [PATCH 22/69] Rearange Drive Constants

---
 src/main/java/frc4388/robot/Constants.java | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 3678f33..272cf88 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -36,14 +36,21 @@ public final class Constants {
         public static final boolean isAuxPIDInverted = false;
 
         /* Drive Configuration */
-        public static final double OPEN_LOOP_RAMP_RATE = 0.1;
+        public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config
+        public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor
         public static final double NEUTRAL_DEADBAND = 0.04;
         public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
             new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01);
-        public static final int CLOSED_LOOP_TIME_MS = 1;
+        public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops
+
+        /* Drive Train Characteristics */
+        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;
+        public static final double TICKS_PER_MOTOR_REV = 2048;
 
         /* PID Constants Drive*/
-        public static final int DRIVE_TIMEOUT_MS = 30;
         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);
@@ -78,13 +85,6 @@ public final class Constants {
 	    public final static int SLOT_TURNING = 2;
 	    public final static int SLOT_MOTION_MAGIC = 3;
         
-        /* Drive Train Characteristics */
-        public static final double TICKS_PER_MOTOR_REV = 2048;
-        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;

From 2756df8e1bbc4828b223a41b23718255662756cc Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Wed, 26 Feb 2020 08:41:10 -0700
Subject: [PATCH 23/69] Simplify Periodic to make it easier to read

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

diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index fca1e7b..d9f1fb2 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -273,14 +273,26 @@ public class Drive extends SubsystemBase {
 
   @Override
   public void periodic() {
+    updateTime();
+    updateAngles();
+    updatePosition();
+    runFalconCooling();
+    updateSmartDashboard();
+  }
+
+  public void updateTime() {
     m_lastTimeMs = m_currentTimeMs;
     m_currentTimeMs = System.currentTimeMillis();
     m_currentTimeSec = m_currentTimeMs / 1000;
     m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs;
+  }
 
+  public void updateAngles() {
     m_lastAngleYaw = m_currentAngleYaw;
     m_currentAngleYaw = getGyroYaw();
+  }
 
+  public void updatePosition() {
     m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
     m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
 
@@ -295,9 +307,6 @@ public class Drive extends SubsystemBase {
     m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               getDistanceInches(m_leftFrontMotor),
                                               -getDistanceInches(m_rightFrontMotor));
-
-    runFalconCooling();
-    updateSmartDashboard();
   }
 
   /**

From 20b73e0e31e18bef9f1c771115a03c8f27b926d5 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Wed, 26 Feb 2020 16:46:22 -0700
Subject: [PATCH 24/69] Fixed Motion Profile Command, still needs testing

---
 src/main/java/frc4388/robot/commands/DrivePositionMPAux.java | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
index 68d9538..0afb152 100644
--- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
@@ -23,6 +23,7 @@ public class DrivePositionMPAux extends CommandBase {
   double m_rampAcc;
   long m_startTime;
   long m_rampRate;
+  int m_counter;
 
   /**
    * Creates a new DrivePositionMPAux.
@@ -54,6 +55,7 @@ public class DrivePositionMPAux extends CommandBase {
     m_targetVel = m_currentVel;
     m_startTime = System.currentTimeMillis();
     m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
+    m_counter = 0;
   }
 
   // Called every m_isRamptime the scheduler runs while the command is scheduled.
@@ -72,6 +74,7 @@ public class DrivePositionMPAux extends CommandBase {
       // Deramp PID
       m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
     }
+    m_counter ++;
   }
 
   // Called once the command ends or is interrupted.
@@ -82,7 +85,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_LOW) {
+    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) {
       return true;
     }
     return false;

From 5bcf6c03905b67521ba521fefd7f8b923cb5cb40 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Wed, 26 Feb 2020 16:48:35 -0700
Subject: [PATCH 25/69] Deleted problematic import statement

---
 src/main/java/frc4388/robot/subsystems/Storage.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 3d460fe..b6eb85e 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -19,7 +19,6 @@ 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;

From 2e3fea423e502f39d889e356ed46e5d969d1257a Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Wed, 26 Feb 2020 17:26:49 -0700
Subject: [PATCH 26/69] Created and Added Solenoid Functionality for Pneumatics
 Subsystem

---
 src/main/java/frc4388/robot/Constants.java    | 10 +++
 .../java/frc4388/robot/RobotContainer.java    | 12 ++-
 .../java/frc4388/robot/subsystems/Drive.java  | 63 ++++---------
 .../frc4388/robot/subsystems/Pneumatics.java  | 89 +++++++++++++++++++
 4 files changed, 123 insertions(+), 51 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/subsystems/Pneumatics.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 272cf88..4af2056 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -156,6 +156,16 @@ public final class Constants {
         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 PneumaticsConstants {
+        public static final int PCM_MODULE_ID = 7;
+
+        public static final int SPEED_SHIFT_FORWARD_ID = 0;
+        public static final int SPEED_SHIFT_REVERSE_ID = 1;
+
+        public static final int COOL_FALCON_FORWARD_ID = 3;
+        public static final int COOL_FALCON_REVERSE_ID = 2;
+    }
   
     public static final class LEDConstants {
         public static final int LED_SPARK_ID = 0;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 2156bd0..d2761a7 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -42,6 +42,7 @@ import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.commands.storageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
+import frc4388.robot.subsystems.Pneumatics;
 import frc4388.robot.subsystems.Storage;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
@@ -56,6 +57,7 @@ import frc4388.utility.controller.XboxController;
 public class RobotContainer {
     /* Subsystems */
     private final Drive m_robotDrive = new Drive();
+    private final Pneumatics m_robotPneumatics = new Pneumatics();
     private final LED m_robotLED = new LED();
     private final Intake m_robotIntake = new Intake();
     private final Shooter m_robotShooter = new Shooter();
@@ -75,6 +77,10 @@ public class RobotContainer {
      * The container for the robot.  Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
+        /* Passing Drive and Pneumatics Subsystems */
+        m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
+        m_robotDrive.passRequiredSubsystem(m_robotPneumatics);
+
         configureButtonBindings();
 
         /* Default Commands */
@@ -120,11 +126,11 @@ public class RobotContainer {
         /* Driver Buttons */
         // sets solenoids into high gear
         new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
 
         // sets solenoids into low gear
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
 
         /* Operator Buttons */
         //TODO: Shooter Buttons
@@ -231,7 +237,7 @@ public class RobotContainer {
      * @param state the gearing of the gearbox (true is high, false is low)
      */
     public void setDriveGearState(boolean state) {
-        m_robotDrive.setShiftState(state);
+        m_robotPneumatics.setShiftState(state);
     }
 
     /**
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index d9f1fb2..bbf917b 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -36,23 +36,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.Constants.PneumaticsConstants;
 import frc4388.robot.Gains;
 
 public class Drive extends SubsystemBase {
-  /* Create Motors, Gyros, Solenoids, etc */
+  /* Create Motors, Gyros, etc */
   public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
   public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
   public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
   public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
   public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
-  public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1);
-  public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2);
   
   /* Drive objects to manage Drive Train */
   public DifferentialDrive m_driveTrain;
   public final DifferentialDriveOdometry m_odometry;
   public Orchestra m_orchestra;
 
+  /* Pneumatics Subsystem */
+  Pneumatics m_pneumaticsSubsystem;
+
   /* Low Gear Gains */
   public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
   public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
@@ -90,7 +92,6 @@ public class Drive extends SubsystemBase {
   SendableChooser<String> m_songChooser = new SendableChooser<String>();
   
   /* Misc */
-  public boolean m_isSpeedShiftHigh;
   String m_currentSong = "";
 
   /**
@@ -276,10 +277,17 @@ public class Drive extends SubsystemBase {
     updateTime();
     updateAngles();
     updatePosition();
-    runFalconCooling();
     updateSmartDashboard();
   }
 
+  /**
+   * Passes subsystem needed.
+   * @param subsystem Subsystem needed.
+   */
+  public void passRequiredSubsystem(Pneumatics subsystem) {
+    m_pneumaticsSubsystem = subsystem;
+  }
+
   public void updateTime() {
     m_lastTimeMs = m_currentTimeMs;
     m_currentTimeMs = System.currentTimeMillis();
@@ -440,47 +448,6 @@ public class Drive extends SubsystemBase {
     m_driveTrain.feedWatchdog();
   }
 
-  /**
-   * Set to high or low gear based on boolean state, true = high, false = low
-   * @param state Chooses between high or low gear
-   */
-  public void setShiftState(boolean state) {
-    if (state == true) {
-			m_speedShift.set(DoubleSolenoid.Value.kReverse);
-    }
-		if (state == false) {
-			m_speedShift.set(DoubleSolenoid.Value.kForward);
-    }
-    setRightMotorGains(state);
-    m_isSpeedShiftHigh = state;
-  }
-
-  /**
-   * Set to open or close solenoid that cools the falcon, true = open, false = close
-   * @param state Chooses between open and close
-   */
-  public void coolFalcon(boolean state) {
-    if (state == true) {
-      m_coolFalcon.set(DoubleSolenoid.Value.kForward);
-    }
-    if (state == false) {
-      m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
-    }
-  }
-
-  /**
-   * 
-   */
-  public void runFalconCooling() {
-    if (m_currentTimeSec % 30 == 0) {
-      coolFalcon(true);
-      SmartDashboard.putBoolean("Solenoid", true);
-    } else if ((m_currentTimeSec - 1) % 30 == 0) {
-      coolFalcon(false);
-      SmartDashboard.putBoolean("Solenoid", false);
-    }
-  }
-
   /**
    * Selects a song to play!
    * @param song The name of the song to be played
@@ -633,7 +600,7 @@ public class Drive extends SubsystemBase {
    * @return The converted value in inches
    */
   public double  ticksToInches(double ticks) {
-    if (m_isSpeedShiftHigh) {
+    if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
       return ticks * DriveConstants.INCHES_PER_TICK_HIGH;
     } else {
       return ticks * DriveConstants.INCHES_PER_TICK_LOW;
@@ -646,7 +613,7 @@ public class Drive extends SubsystemBase {
    * @return The converted value in ticks
    */
   public double inchesToTicks(double inches) {
-    if (m_isSpeedShiftHigh) {
+    if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) {
       return inches * DriveConstants.TICKS_PER_INCH_HIGH;
     } else {
       return inches * DriveConstants.TICKS_PER_INCH_LOW;
diff --git a/src/main/java/frc4388/robot/subsystems/Pneumatics.java b/src/main/java/frc4388/robot/subsystems/Pneumatics.java
new file mode 100644
index 0000000..1c373ed
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/Pneumatics.java
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.Constants.PneumaticsConstants;
+
+public class Pneumatics extends SubsystemBase {
+  /* Create Solenoids */
+  public DoubleSolenoid m_speedShift = new DoubleSolenoid(  PneumaticsConstants.PCM_MODULE_ID, 
+                                                            PneumaticsConstants.SPEED_SHIFT_FORWARD_ID,
+                                                            PneumaticsConstants.SPEED_SHIFT_REVERSE_ID );
+
+  public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(  PneumaticsConstants.PCM_MODULE_ID, 
+                                                            PneumaticsConstants.COOL_FALCON_FORWARD_ID, 
+                                                            PneumaticsConstants.COOL_FALCON_REVERSE_ID );
+
+  /* Get Drive Subsystem */
+  Drive m_driveSubsystem;
+
+  public boolean m_isSpeedShiftHigh;
+
+  /**
+   * Creates a new Pneumatics.
+   */
+  public Pneumatics() {
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    runFalconCooling();
+  }
+  
+  /**
+   * Passes subsystem needed.
+   * @param subsystem Subsystem needed.
+   */
+  public void passRequiredSubsystem(Drive subsystem) {
+    m_driveSubsystem = subsystem;
+  }
+
+  /**
+   * Set to high or low gear based on boolean state, true = high, false = low
+   * @param state Chooses between high or low gear
+   */
+  public void setShiftState(boolean state) {
+    if (state == true) {
+			m_speedShift.set(DoubleSolenoid.Value.kReverse);
+    }
+		if (state == false) {
+			m_speedShift.set(DoubleSolenoid.Value.kForward);
+    }
+    m_driveSubsystem.setRightMotorGains(state);
+    m_isSpeedShiftHigh = state;
+  }
+
+  /**
+   * Set to open or close solenoid that cools the falcon, true = open, false = close
+   * @param state Chooses between open and close
+   */
+  public void coolFalcon(boolean state) {
+    if (state == true) {
+      m_coolFalcon.set(DoubleSolenoid.Value.kForward);
+    }
+    if (state == false) {
+      m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
+    }
+  }
+
+  /**
+   * Runs coolFalcon every 30 seconds for 1 second.
+   */
+  public void runFalconCooling() {
+    if (m_driveSubsystem.m_currentTimeSec % 30 == 0) {
+      coolFalcon(true);
+    } else if ((m_driveSubsystem.m_currentTimeSec - 1) % 30 == 0) {
+      coolFalcon(false);
+    }
+  }
+}

From 1cc3aef303fe28f12395c5de89b9527f30a587a9 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Wed, 26 Feb 2020 17:31:57 -0700
Subject: [PATCH 27/69] sdfghl

---
 .../java/frc4388/robot/RobotContainer.java    | 16 +++----
 .../robot/commands/ShootFireGroup.java        |  6 +--
 .../robot/commands/ShootPrepGroup.java        |  4 +-
 .../commands/ShooterVelocityControlPID.java   |  2 +-
 .../frc4388/robot/commands/StorageIntake.java |  7 ++-
 .../robot/commands/StorageIntakeFinal.java    | 47 -------------------
 .../robot/commands/StorageIntakeGroup.java    | 28 -----------
 .../robot/commands/StoragePrepAim.java        |  4 +-
 .../robot/commands/StoragePrepIntake.java     |  6 ++-
 .../frc4388/robot/commands/TrackTarget.java   |  2 +-
 .../frc4388/robot/subsystems/Storage.java     |  4 +-
 11 files changed, 24 insertions(+), 102 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
 delete mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeGroup.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index b65ee53..8d04785 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -40,7 +40,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.commands.StorageIntake;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
@@ -57,6 +57,7 @@ import frc4388.robot.subsystems.LED;
 import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.commands.StorageOutake;
 import frc4388.robot.commands.StoragePrepAim;
+import frc4388.robot.commands.StoragePrepIntake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.LimeLight;
@@ -113,8 +114,6 @@ public class RobotContainer {
         m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
         // moves the drum not
         m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
-        //turns limelight off
-        //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
 
     }
 
@@ -145,10 +144,7 @@ public class RobotContainer {
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
-            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
-            //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity())))
-            //.whenReleased(new RunCommand(() -> m_robotShooterAim.limeOff()))
-            //.whenReleased(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(0)));
+
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
             .whenPressed(new RunExtenderOutIn(m_robotIntake));
@@ -160,12 +156,12 @@ public class RobotContainer {
 
         // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim));
-            //.whenPressed(new StoragePrepAim(m_robotStorage));
+            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
+            .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
-            .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage));
+            .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage));
             
         //Runs storage to outtake
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
index 855fc48..7d23c65 100644
--- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -25,10 +25,10 @@ public class ShootFireGroup extends ParallelRaceGroup {
    */
   public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     addCommands(
-      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(7000, m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
+      new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
       new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
-      new HoldTarget(m_shooter, m_shooterAim)
-      //new StorageRun(m_storage)
+      new HoldTarget(m_shooter, m_shooterAim),
+      new StorageRun(m_storage)
     );
   }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
index 5d60abb..7407365 100644
--- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
@@ -26,8 +26,8 @@ public class ShootPrepGroup extends ParallelCommandGroup {
   public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     addCommands(
       new TrackTarget(m_shooter, m_shooterAim),
-      new ShooterVelocityControlPID(m_shooter)
-      //new StoragePrepAim(m_storage)
+      new ShooterVelocityControlPID(m_shooter),
+      new StoragePrepAim(m_storage)
     );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index a2a1b77..e84813c 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -33,7 +33,7 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_shooter.runDrumShooterVelocityPID(7000, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
     m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
   }
 
diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/StorageIntake.java
index 30d73ce..b382a40 100644
--- a/src/main/java/frc4388/robot/commands/StorageIntake.java
+++ b/src/main/java/frc4388/robot/commands/StorageIntake.java
@@ -38,12 +38,11 @@ public class StorageIntake extends CommandBase {
   public void execute() {
 
     if (m_storage.getBeam(0)){
-      m_storage.setStoragePID(m_storage.getEncoderPos() + StorageConstants.STORAGE_PARTIAL_BALL);
-      m_intake.runExtender(-IntakeConstants.EXTENDER_SPEED); 
+      m_storage.runStorage(StorageConstants.STORAGE_SPEED);
       intook = true;
     }
     else{
-      m_intake.runExtender(IntakeConstants.EXTENDER_SPEED); 
+      m_storage.runStorage(0); 
     }
   }
 
@@ -55,7 +54,7 @@ public class StorageIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(0) && m_storage.getBeam(1) && intook){
+    if (!m_storage.getBeam(0) && intook){
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
deleted file mode 100644
index 7ac5f46..0000000
--- a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java
+++ /dev/null
@@ -1,47 +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.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.StorageConstants;
-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() + StorageConstants.STORAGE_FULL_BALL);
-    }
-  }
-
-  // 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
deleted file mode 100644
index 4636edc..0000000
--- a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java
+++ /dev/null
@@ -1,28 +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.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/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/StoragePrepAim.java
index 376a92b..7af0252 100644
--- a/src/main/java/frc4388/robot/commands/StoragePrepAim.java
+++ b/src/main/java/frc4388/robot/commands/StoragePrepAim.java
@@ -29,7 +29,7 @@ public class StoragePrepAim extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_storage.getBeam(2) == false){
+    if (m_storage.getBeam(1) == false){
       m_storage.runStorage(StorageConstants.STORAGE_SPEED);
     }
     else{
@@ -45,7 +45,7 @@ public class StoragePrepAim extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(2)){
+    if (m_storage.getBeam(1)){
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
index 3321027..3a94353 100644
--- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
+++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
@@ -15,6 +15,7 @@ import frc4388.robot.subsystems.Storage;
 public class StoragePrepIntake extends CommandBase {
   public Intake m_intake;
   public Storage m_storage;
+  public double startTime;
 
   /**
    * Creates a new StoragePrepIntake.
@@ -29,12 +30,13 @@ public class StoragePrepIntake extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    startTime = System.currentTimeMillis();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_storage.getBeam(1) == false){
+    if (!m_storage.getBeam(0) && System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed
       m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
     }
     else{
@@ -50,7 +52,7 @@ public class StoragePrepIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(1)){
+    if (m_storage.getBeam(0)){
       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 647341b..e7c3c08 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -73,7 +73,7 @@ public class TrackTarget extends CommandBase {
       } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
         turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
       }
-      m_shooterAim.runShooterWithInput(turnAmount / 3);
+      m_shooterAim.runShooterWithInput(turnAmount);
 
       // Finding Distance
       distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 5dbcc44..74b2484 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -98,10 +98,10 @@ public class Storage extends SubsystemBase {
 
   /*
   If pressing aim
+    run unti; hitting top beam
+  else
     Run until hitting bottom beam
     dont run intake if balls not at bottom
-  else
-    run unti; hitting top beam
   
   2 beamms total
   */

From afaae3ca1d145c1e03e716dc7b490481fc252791 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Wed, 26 Feb 2020 17:36:50 -0700
Subject: [PATCH 28/69] yuh2

---
 .../commands/DriveWithJoystickUsingDeadAssistPID.java | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
index 12d605c..5a68a9f 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
@@ -12,10 +12,12 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpiutil.math.MathUtil;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 import frc4388.utility.controller.IHandController;
 
 public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   Drive m_drive;
+  Pneumatics m_pneumatics;
   double m_targetGyro, m_currentGyro;
   double m_stopPos;
   long m_currTime, m_deltaTime;
@@ -44,14 +46,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
    * 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".
    * Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
-   * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   * @param subsystemDrive pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
    * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
    * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
    * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
    */
-  public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
+  public DriveWithJoystickUsingDeadAssistPID(Drive subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) {
     // Use addRequirements() here to declare subsystem dependencies.
-    m_drive = subsystem;
+    m_drive = subsystemDrive;
+    m_pneumatics = subsystemPneumatics;
     m_controller = controller;
     addRequirements(m_drive);
   }
@@ -96,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    if (m_drive.m_isSpeedShiftHigh) {
+    if (m_pneumatics.m_isSpeedShiftHigh) {
       runDriveWithInput(moveOutput, steerInput);
       resetGyroTarget();
     }

From 3a3d10a3823425bc2b42a5c5da09a4915e11b80c Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Wed, 26 Feb 2020 20:29:01 -0700
Subject: [PATCH 29/69] Work on Broken PIDs

---
 src/main/java/frc4388/robot/Constants.java    |  4 +--
 src/main/java/frc4388/robot/Robot.java        |  2 +-
 .../java/frc4388/robot/RobotContainer.java    |  8 +++--
 .../robot/commands/DrivePositionMPAux.java    |  6 ++--
 .../robot/commands/DriveWithJoystick.java     |  4 +--
 .../frc4388/robot/commands/TurnDegrees.java   |  2 +-
 .../java/frc4388/robot/subsystems/Drive.java  | 34 +++++++++----------
 7 files changed, 31 insertions(+), 29 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 4af2056..89d2b77 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -30,7 +30,7 @@ public final class Constants {
         public static final int PIGEON_ID = 6;
 
         /* Drive Inversions */
-        public static final boolean isRightMotorInverted = false;
+        public static final boolean isRightMotorInverted = true;
         public static final boolean isLeftMotorInverted = false;
         public static final boolean isRightArcadeInverted = false;
         public static final boolean isAuxPIDInverted = false;
@@ -53,7 +53,7 @@ public final class Constants {
         /* PID Constants Drive*/
         public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
         public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
-        public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
+        public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55);
         public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
         public static final int DRIVE_CRUISE_VELOCITY = 20000;
         public static final int DRIVE_ACCELERATION = 7000;
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 1029754..4f894c9 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -105,7 +105,7 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
-    m_robotContainer.setDriveGearState(true);
+    //m_robotContainer.setDriveGearState(true);
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
     
     // This makes sure that the autonomous stops running when
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index d2761a7..a51873d 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -27,6 +27,7 @@ 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.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
@@ -39,6 +40,7 @@ import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
 import frc4388.robot.commands.TrackTarget;
+import frc4388.robot.commands.TurnDegrees;
 import frc4388.robot.commands.storageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
@@ -109,11 +111,11 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new TurnDegrees(90, m_robotDrive));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
@@ -183,7 +185,7 @@ public class RobotContainer {
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
 
         // return new InstantCommand();
-        return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
+        return new DrivePositionMPAux(m_robotDrive, 12.0, 12.0, 2, 100.0);
     }
 
     TrajectoryConfig getTrajectoryConfig() {
diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
index 0afb152..ea76f96 100644
--- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
@@ -29,12 +29,12 @@ public class DrivePositionMPAux extends CommandBase {
    * Creates a new DrivePositionMPAux.
    * 
    * @param subsystem The drive subsystem
-   * @param cruiseVel The target velocity for the motors in units
+   * @param cruiseVel The target velocity for the motors in in/s
    * @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) {
+  public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
     m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
@@ -86,7 +86,7 @@ public class DrivePositionMPAux extends CommandBase {
   @Override
   public boolean isFinished() {
     if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) {
-      return true;
+      //return true;
     }
     return false;
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index f51621a..2811fe1 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -38,8 +38,8 @@ public class DriveWithJoystick extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double moveInput = -m_controller.getLeftYAxis();
-    double steerInput = m_controller.getRightXAxis();
+    double moveInput = m_controller.getRightXAxis();
+    double steerInput = -m_controller.getLeftYAxis();
     double moveOutput = 0;
     double steerOutput = 0;
     if (moveInput >= 0){
diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java
index 2b47050..52eb5dc 100644
--- a/src/main/java/frc4388/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java
@@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
+    if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) {
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index bbf917b..422d2c7 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -106,13 +106,6 @@ public class Drive extends SubsystemBase {
     m_pigeon.configFactoryDefault();
     resetGyroYaw();
 
-    /* flip input so forward becomes back, etc */
-    m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
-    m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
-    m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
-    m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
-    m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
-
     /* Config Open Loop Ramp so we don't make sudden output changes */
     m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -120,10 +113,10 @@ public class Drive extends SubsystemBase {
     m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* Config Supply Current Limit (Use only for debugging) */
-    m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
-    m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
-    m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
-    m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
 
     /* Config deadbands so that  */
     m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -252,6 +245,13 @@ public class Drive extends SubsystemBase {
 
     /* Set up Orchestra */
     m_orchestra = new Orchestra();
+
+    /* flip input so forward becomes back, etc */
+    m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
+    m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
+    m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
+    m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
+    //m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
     
     /* Set up music for drive train */
     m_orchestra.addInstrument(m_leftBackMotor);
@@ -322,7 +322,7 @@ public class Drive extends SubsystemBase {
    * using the Differential Drive class to manage the two inputs
    */
   public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(steer, move);
+    m_driveTrain.arcadeDrive(move, steer);
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
   }
@@ -723,8 +723,8 @@ public class Drive extends SubsystemBase {
       //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 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());
@@ -756,9 +756,9 @@ public class Drive extends SubsystemBase {
       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));
+      SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
       //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
-      //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
+      SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
       //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
       //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
 
@@ -766,7 +766,7 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("Odometry Heading", getHeading());
 
       //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
-      //SmartDashboard.putNumber("Delta Time", m_deltaTime);
+      SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
 
       if (m_currentSong != m_songChooser.getSelected()){
         m_currentSong = m_songChooser.getSelected();

From 5a7939f5fac3fe846eff3b434534d684114c90eb Mon Sep 17 00:00:00 2001
From: Kyra Rivera <101209@psdschools.org>
Date: Thu, 27 Feb 2020 16:27:02 -0700
Subject: [PATCH 30/69] Added Autonomous Command

---
 .../java/frc4388/robot/RobotContainer.java    | 31 +++++++++++++++++--
 1 file changed, 28 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index d2761a7..9857aa8 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -21,8 +21,10 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
@@ -39,6 +41,8 @@ import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
 import frc4388.robot.commands.TrackTarget;
+import frc4388.robot.commands.TurnDegrees;
+import frc4388.robot.commands.Wait;
 import frc4388.robot.commands.storageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
@@ -181,11 +185,32 @@ public class RobotContainer {
         RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
         // Run path following command, then stop at the end.
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+       
+        //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
+        //This assumes that we are positioned against the right wall with our shooter facing the target.
+        return new SequentialCommandGroup(new Wait(2, m_robotDrive), 
+                                    //add aim command
+                                    //add shooter command
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0, 0.0),
+                                    new ParallelCommandGroup(
+                                        new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0, 0.0)),
+                                    new ParallelCommandGroup(
+                                         new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0, 0)),
+                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                    //add aim command 
+                                    //add shooter command
+//Below this would be the picking up additional balls outside of those in the trench directly behind us
 
-        // return new InstantCommand();
-        return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
+                                    new TurnDegrees(-150, m_robotDrive),
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0, 0.0),
+                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                    new TurnDegrees(75, m_robotDrive),
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0, 0.0),
+                                    new TurnDegrees(-45, m_robotDrive),
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0, 0.0));
     }
-
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(
             DriveConstants.MAX_SPEED_METERS_PER_SECOND,

From 313ae0684ccbc7d5a511045c3ef41215c4875eac Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Thu, 27 Feb 2020 16:37:44 -0700
Subject: [PATCH 31/69] Fixed Errors in RobotContainer

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 5b59a6a..18dee5d 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -43,6 +43,7 @@ import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
 import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.commands.TurnDegrees;
+import frc4388.robot.commands.Wait;
 import frc4388.robot.commands.storageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
@@ -191,25 +192,25 @@ public class RobotContainer {
         return new SequentialCommandGroup(new Wait(2, m_robotDrive), 
                                     //add aim command
                                     //add shooter command
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0, 0.0),
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0),
                                     new ParallelCommandGroup(
                                         new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0, 0.0)),
+                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
                                     new ParallelCommandGroup(
                                          new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0, 0)),
+                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
                                     new StorageIntakeGroup(m_robotIntake, m_robotStorage),
                                     //add aim command 
                                     //add shooter command
 //Below this would be the picking up additional balls outside of those in the trench directly behind us
 
                                     new TurnDegrees(-150, m_robotDrive),
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0, 0.0),
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0),
                                     new StorageIntakeGroup(m_robotIntake, m_robotStorage),
                                     new TurnDegrees(75, m_robotDrive),
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0, 0.0),
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0),
                                     new TurnDegrees(-45, m_robotDrive),
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0, 0.0));
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0));
     }
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(

From 82286cb180137fcf00fb1df8386027cbad69e47b Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Thu, 27 Feb 2020 17:14:32 -0700
Subject: [PATCH 32/69] Fixed Drive With Joysticks

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

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 2811fe1..01375a4 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Drive;
 import frc4388.utility.controller.IHandController;
@@ -42,22 +43,23 @@ public class DriveWithJoystick extends CommandBase {
     double steerInput = -m_controller.getLeftYAxis();
     double moveOutput = 0;
     double steerOutput = 0;
-    if (moveInput >= 0){
-      moveOutput = -Math.cos(1.571*moveInput)+1;
+    if (steerInput >= 0){
+      steerOutput = -Math.cos(1.571*steerInput)+1;
     } else {
-      moveOutput = Math.cos(1.571*moveInput)-1;
+      steerOutput = Math.cos(1.571*steerInput)-1;
     }
 
-    double cosMultiplier = .55;
+    double cosMultiplier = 1.0;
     double deadzone = .1;
-    if (steerInput > 0){
-      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
-    } else if (steerInput < 0) {
-      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
+    if (moveInput > 0){
+      moveOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*moveInput) + cosMultiplier;
+    } else if (moveInput < 0) {
+      moveOutput = (cosMultiplier - deadzone) * Math.cos(1.571*moveInput) - cosMultiplier;
     } else {
-      steerOutput = 0;
+      moveOutput = 0;
     }
-
+    
+    SmartDashboard.putNumber("Steer Output Test", moveOutput);
     m_drive.driveWithInput(moveOutput, steerOutput);
   }
 

From c572182678625990b39ca0dad422de76ba79ab43 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Fri, 28 Feb 2020 17:38:15 -0700
Subject: [PATCH 33/69] All low gear PIDs WORKY, test auto command WORKY

---
 src/main/java/frc4388/robot/Constants.java    |  4 +--
 .../java/frc4388/robot/RobotContainer.java    | 34 ++++++++++---------
 .../robot/commands/DrivePositionMPAux.java    |  6 ++--
 .../commands/DriveStraightAtVelocityPID.java  |  2 +-
 .../commands/DriveStraightToPositionPID.java  |  2 +-
 .../frc4388/robot/commands/TurnDegrees.java   |  2 +-
 6 files changed, 26 insertions(+), 24 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 5fb5bc2..5054477 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -55,8 +55,8 @@ public final class Constants {
         public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
         public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55);
         public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
-        public static final int DRIVE_CRUISE_VELOCITY = 20000;
-        public static final int DRIVE_ACCELERATION = 7000;
+        public static final int DRIVE_CRUISE_VELOCITY = 25000;
+        public static final int DRIVE_ACCELERATION = 21000;
 
         public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5);
         public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 1b5b04c..4576358 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -30,6 +30,8 @@ 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.DriveStraightToPositionMM;
+import frc4388.robot.commands.DriveStraightToPositionPID;
 import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
@@ -114,19 +116,19 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new TurnDegrees(90, m_robotDrive));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, -192));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
+            .whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new DrivePositionMPAux(m_robotDrive, 12.0, 3.0, 1, 36.0));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
      
         /* Driver Buttons */
         // sets solenoids into high gear
@@ -192,25 +194,25 @@ public class RobotContainer {
         return new SequentialCommandGroup(new Wait(2, m_robotDrive), 
                                     //add aim command
                                     //add shooter command
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0),
-                                    new ParallelCommandGroup(
+                                    new DriveStraightToPositionMM(m_robotDrive, 48.0),
+                                    /*new ParallelCommandGroup(
                                         new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
+                                            new DriveStraightToPositionMM(m_robotDrive, 36.0)),
                                     new ParallelCommandGroup(
                                          new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
-                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                            new DriveStraightToPositionMM(m_robotDrive, 36.0)),
+                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),*/
                                     //add aim command 
                                     //add shooter command
 //Below this would be the picking up additional balls outside of those in the trench directly behind us
 
-                                    new TurnDegrees(-150, m_robotDrive),
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0),
-                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),
-                                    new TurnDegrees(75, m_robotDrive),
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0),
-                                    new TurnDegrees(-45, m_robotDrive),
-                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0));
+                                    new TurnDegrees(m_robotDrive, -90),
+                                    new DriveStraightToPositionMM(m_robotDrive, 96.0));//,
+                                    //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                    //new TurnDegrees(m_robotDrive, 75),
+                                    //new DriveStraightToPositionMM(m_robotDrive, 18.0),
+                                    //new TurnDegrees(m_robotDrive, -45),
+                                    //new DriveStraightToPositionMM(m_robotDrive, 12.0));
     }
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(
diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
index ea76f96..68d8390 100644
--- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
+++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java
@@ -66,13 +66,13 @@ public class DrivePositionMPAux extends CommandBase {
     if (System.currentTimeMillis() - m_startTime < m_rampRate) {
       // Ramping
       m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
-      m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+      m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
     } else if (m_targetPos - m_currentPos > m_rampDist) {
       // Cruising
-      m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
+      m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro);
     } else {
       // Deramp PID
-      m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
+      m_drive.runDrivePositionPID(m_targetPos, m_targetGyro);
     }
     m_counter ++;
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
index 14cc97e..c79ccbc 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
   @Override
   public void execute() {
     //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
-    m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+    m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro);
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
index 949a4bf..9d84087 100644
--- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
@@ -58,7 +58,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){
+    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){
       return true;
     } else {
       i++;
diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java
index 52eb5dc..83630dd 100644
--- a/src/main/java/frc4388/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java
@@ -24,7 +24,7 @@ public class TurnDegrees extends CommandBase {
   /**
    * Creates a new TurnDeg.
    */
-  public TurnDegrees(double targetAngle, Drive subsystem) {
+  public TurnDegrees(Drive subsystem, double targetAngle) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_targetAngle = targetAngle;

From d83d21e062c51f84f943d0c73f3df9da7fdab72e Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Fri, 28 Feb 2020 18:11:11 -0700
Subject: [PATCH 34/69] Fixed Drive Train the right way, switched gear buttons
 for Josh

---
 .../java/frc4388/robot/RobotContainer.java    |  4 +--
 .../robot/commands/DriveWithJoystick.java     | 28 +++++++++++--------
 .../java/frc4388/robot/subsystems/Drive.java  |  2 +-
 3 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 4576358..20518cc 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -133,11 +133,11 @@ public class RobotContainer {
         /* Driver Buttons */
         // sets solenoids into high gear
         new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
 
         // sets solenoids into low gear
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
+            .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
 
         /* Operator Buttons */
         //TODO: Shooter Buttons
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 01375a4..4cd0d0e 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -39,26 +39,32 @@ public class DriveWithJoystick extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double moveInput = m_controller.getRightXAxis();
-    double steerInput = -m_controller.getLeftYAxis();
+    double moveInput = -m_controller.getLeftYAxis();
+    double steerInput = m_controller.getRightXAxis();
     double moveOutput = 0;
     double steerOutput = 0;
-    if (steerInput >= 0){
-      steerOutput = -Math.cos(1.571*steerInput)+1;
+    if (moveInput >= 0){
+      moveOutput = -Math.cos(1.571*moveInput)+1;
     } else {
-      steerOutput = Math.cos(1.571*steerInput)-1;
+      moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
     double cosMultiplier = 1.0;
     double deadzone = .1;
-    if (moveInput > 0){
-      moveOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*moveInput) + cosMultiplier;
-    } else if (moveInput < 0) {
-      moveOutput = (cosMultiplier - deadzone) * Math.cos(1.571*moveInput) - cosMultiplier;
+    if (steerInput > 0){
+      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
+    } else if (steerInput < 0) {
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
     } else {
-      moveOutput = 0;
+      steerOutput = 0;
     }
-    
+
+    if (moveOutput > 0.5) {
+      moveOutput = 0.5;
+    } else if(moveOutput < -0.5) {
+      moveOutput = -0.5;
+    }
+
     SmartDashboard.putNumber("Steer Output Test", moveOutput);
     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 422d2c7..f4541dc 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -322,7 +322,7 @@ public class Drive extends SubsystemBase {
    * using the Differential Drive class to manage the two inputs
    */
   public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(move, steer);
+    m_driveTrain.arcadeDrive(steer, move);
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
   }

From ad50840f7c987c87ee55b67cb0dbe0723c1ad62d Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Fri, 28 Feb 2020 19:13:38 -0700
Subject: [PATCH 35/69] Applied 80% Output Limit, Fixed PID 1/2 Distance
 Problem

---
 .../robot/commands/DriveStraightToPositionMM.java |  2 +-
 .../commands/DriveStraightToPositionPID.java      |  2 +-
 .../frc4388/robot/commands/DriveWithJoystick.java | 15 +++++++++++----
 3 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java
index c56a36b..925f07a 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_LOW;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
     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 9d84087..c31944e 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_LOW;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 4cd0d0e..1802f22 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -58,11 +58,18 @@ public class DriveWithJoystick extends CommandBase {
     } else {
       steerOutput = 0;
     }
+    double tempOutputLimit = 0.8;
 
-    if (moveOutput > 0.5) {
-      moveOutput = 0.5;
-    } else if(moveOutput < -0.5) {
-      moveOutput = -0.5;
+    if (moveOutput > tempOutputLimit) {
+      moveOutput = tempOutputLimit;
+    } else if(moveOutput < -tempOutputLimit) {
+      moveOutput = -tempOutputLimit;
+    }
+
+    if (steerOutput > tempOutputLimit) {
+      steerOutput = tempOutputLimit;
+    } else if(steerOutput < -tempOutputLimit) {
+      steerOutput = -tempOutputLimit;
     }
 
     SmartDashboard.putNumber("Steer Output Test", moveOutput);

From b45f66b0237e87d3cb3f46730135d30f3a7b4670 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 28 Feb 2020 20:05:56 -0700
Subject: [PATCH 36/69] Messing with Turret and storage

Not completly sure why the shooting actions arent happening in the right order
---
 src/main/java/frc4388/robot/Constants.java    |  8 ++++----
 .../java/frc4388/robot/RobotContainer.java    | 20 +++++++++++++------
 .../frc4388/robot/commands/HoldTarget.java    |  6 +++---
 .../robot/commands/ShootFireGroup.java        |  2 +-
 .../commands/ShooterVelocityControlPID.java   |  5 +++++
 .../frc4388/robot/commands/StorageIntake.java |  4 ++--
 .../robot/commands/StoragePositionPID.java    | 17 ++++++++++------
 .../robot/commands/StoragePrepAim.java        |  7 +++++--
 .../robot/commands/StoragePrepIntake.java     |  5 +++--
 .../frc4388/robot/commands/TrackTarget.java   | 10 ++++++----
 .../java/frc4388/robot/subsystems/Drive.java  |  2 +-
 .../frc4388/robot/subsystems/LimeLight.java   |  4 ++--
 .../frc4388/robot/subsystems/Shooter.java     | 10 ++++++++++
 .../frc4388/robot/subsystems/ShooterAim.java  | 10 +++++++++-
 .../frc4388/robot/subsystems/Storage.java     | 10 +++++++++-
 15 files changed, 85 insertions(+), 35 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 1987d58..02b6f7b 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -120,7 +120,7 @@ public final class Constants {
         public static final int SHOOTER_PID_LOOP_IDX = 1;
         public static final int SHOOTER_TIMEOUT_MS = 30;
         //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
-        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0, 0.0453, 0, 1.0);
+        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 0, 1.0);
         public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
         public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
         public static final double SHOOTER_TURRET_MIN = -1.0;
@@ -130,7 +130,7 @@ public final class Constants {
     }
     
     public static final class ClimberConstants {
-        public static final int CLIMBER_SPARK_ID = 10;
+        public static final int CLIMBER_SPARK_ID = -10;
     }
   
     public static final class LevelerConstants {
@@ -138,7 +138,7 @@ public final class Constants {
     }
   
     public static final class StorageConstants {
-        public static final int STORAGE_CAN_ID = -1;
+        public static final int STORAGE_CAN_ID = 11;
         public static final double STORAGE_PARTIAL_BALL = 2;
         public static final double STORAGE_FULL_BALL = 7;
         public static final double STORAGE_SPEED = 0.5;
@@ -159,7 +159,7 @@ public final class Constants {
 
         /* PID Gains */
         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 Gains STORAGE_GAINS = new Gains(1, 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 76b8ed1..dbb29f6 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -41,7 +41,6 @@ import frc4388.robot.commands.StorageIntake;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
 import frc4388.robot.commands.RunIntakeWithTriggers;
-import frc4388.robot.commands.StorageIntakeGroup;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
@@ -105,7 +104,7 @@ public class RobotContainer {
         // drives intake with input from triggers on the opperator controller
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
         // runs the turret with joystick
-        m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
+        m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
         // moves the drum not
         m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
         // drives climber with input from triggers on the opperator controller
@@ -114,6 +113,8 @@ public class RobotContainer {
         m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+        //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
+        //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
 
     /**
@@ -149,15 +150,18 @@ public class RobotContainer {
         new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
 
+
         /* Operator Buttons */
 
         // shoots until released
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
+            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
 
         // shoots one ball
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
-            .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false);
+            .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false)
+            .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
 
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -171,7 +175,9 @@ public class RobotContainer {
         // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
+            //.whileHeld(new TrackTarget(m_robotShooter,m_robotShooterAim))
             .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
+            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
@@ -182,10 +188,12 @@ public class RobotContainer {
             .whileHeld(new StorageOutake(m_robotStorage));
 
         new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
-            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage));
+            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.2)))
+            .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
 
         new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity())));
+            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.2)))
+            .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
     }
 
     /**
diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java
index cbe43cf..bbbe656 100644
--- a/src/main/java/frc4388/robot/commands/HoldTarget.java
+++ b/src/main/java/frc4388/robot/commands/HoldTarget.java
@@ -49,7 +49,8 @@ public class HoldTarget extends CommandBase {
   @Override
   public void initialize() {
       //Vision Processing Mode
-    LimeLight.limeOn();
+      NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+      NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
 
@@ -88,8 +89,7 @@ public class HoldTarget extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-      //Drive Camera Mode
-    LimeLight.limeOff();
+
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
index 7d23c65..10eb073 100644
--- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -28,7 +28,7 @@ public class ShootFireGroup extends ParallelRaceGroup {
       new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
       new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
       new HoldTarget(m_shooter, m_shooterAim),
-      new StorageRun(m_storage)
+      new StoragePositionPID(m_storage)
     );
   }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index e84813c..a5e0b5a 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Shooter;
 
@@ -35,6 +36,8 @@ public class ShooterVelocityControlPID extends CommandBase {
   public void execute() {
     m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
     m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
+    SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
+    SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
   }
 
   // Called once the command ends or is interrupted.
@@ -49,9 +52,11 @@ public class ShooterVelocityControlPID extends CommandBase {
     double upperBound = m_targetVel + 300;
     double lowerBound = m_targetVel - 300;
     if (m_actualVel < upperBound && m_actualVel > lowerBound){
+      SmartDashboard.putBoolean("ShooterVelocityPID Finished", true);
       return true;
     }
     else{
+      SmartDashboard.putBoolean("ShooterVelocityPID Finished", false);
       return false;
     }
   }
diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/StorageIntake.java
index b382a40..69de480 100644
--- a/src/main/java/frc4388/robot/commands/StorageIntake.java
+++ b/src/main/java/frc4388/robot/commands/StorageIntake.java
@@ -37,7 +37,7 @@ public class StorageIntake extends CommandBase {
   @Override
   public void execute() {
 
-    if (m_storage.getBeam(0)){
+    if (!m_storage.getBeam(0)){
       m_storage.runStorage(StorageConstants.STORAGE_SPEED);
       intook = true;
     }
@@ -54,7 +54,7 @@ public class StorageIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(0) && intook){
+    if (m_storage.getBeam(0) && intook){
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
index 1270d3c..9536e24 100644
--- a/src/main/java/frc4388/robot/commands/StoragePositionPID.java
+++ b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
@@ -8,30 +8,31 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
 
-
 public class StoragePositionPID extends CommandBase {
-  Storage m_storage;
+  public Storage m_storage;
+  double startPos;
   /**
    * Creates a new StoragePositionPID.
    */
-  public StoragePositionPID(Storage storage) {
-    m_storage = storage;
+  public StoragePositionPID(Storage subsystem) {
+    m_storage = subsystem;
     addRequirements(m_storage);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    startPos = m_storage.getEncoderPos();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL));
+    System.err.println("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooiujgxzxfghjkiujsdasdgioiedsdjkl");
+    m_storage.runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL);
   }
 
   // Called once the command ends or is interrupted.
@@ -42,6 +43,10 @@ public class StoragePositionPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
+    if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
+    {
+      return true;
+    }
     return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/StoragePrepAim.java
index 7af0252..2782c7c 100644
--- a/src/main/java/frc4388/robot/commands/StoragePrepAim.java
+++ b/src/main/java/frc4388/robot/commands/StoragePrepAim.java
@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
@@ -29,7 +30,7 @@ public class StoragePrepAim extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_storage.getBeam(1) == false){
+    if (m_storage.getBeam(1)){
       m_storage.runStorage(StorageConstants.STORAGE_SPEED);
     }
     else{
@@ -45,9 +46,11 @@ public class StoragePrepAim extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(1)){
+    if (!m_storage.getBeam(1)){
+      SmartDashboard.putBoolean("StoragePrepAim Finished", true);
       return true;
     }
+    SmartDashboard.putBoolean("StoragePrepAim Finished", false);
     return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
index 3a94353..643c858 100644
--- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
+++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
@@ -36,7 +36,8 @@ public class StoragePrepIntake extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (!m_storage.getBeam(0) && System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed
+    System.err.println(m_storage.getBeam(0));
+    if (m_storage.getBeam(0)){//&& System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed
       m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
     }
     else{
@@ -52,7 +53,7 @@ public class StoragePrepIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_storage.getBeam(0)){
+    if (!m_storage.getBeam(0)){
       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 e7c3c08..5db2669 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -51,7 +51,8 @@ public class TrackTarget extends CommandBase {
   @Override
   public void initialize() {
     // Vision Processing Mode
-    LimeLight.limeOn();
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -93,17 +94,18 @@ public class TrackTarget extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    // Drive Camera Mode
-    LimeLight.limeOff();
+
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (xAngle < 1 && xAngle > -1)
+    if (xAngle < 1 && xAngle > -1 && target == 1)
     {
+      SmartDashboard.putBoolean("TrackTarget Finished", true);
       return true;
     }
+    SmartDashboard.putBoolean("TrackTarget Finished", false);
     return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index fca1e7b..3a87531 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -110,7 +110,7 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
     m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
     m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
-    m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
+   // m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
 
     /* Config Open Loop Ramp so we don't make sudden output changes */
     m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
diff --git a/src/main/java/frc4388/robot/subsystems/LimeLight.java b/src/main/java/frc4388/robot/subsystems/LimeLight.java
index 70c53ae..d8e39a4 100644
--- a/src/main/java/frc4388/robot/subsystems/LimeLight.java
+++ b/src/main/java/frc4388/robot/subsystems/LimeLight.java
@@ -18,12 +18,12 @@ public class LimeLight extends SubsystemBase {
 
   }
 
-  public static void limeOff(){
+  public void limeOff(){
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
   }
   
-  public static void limeOn(){
+  public void limeOn(){
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index 2180447..c8bb07d 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -13,9 +13,11 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
 import com.revrobotics.ControlType;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.Joystick;
@@ -48,6 +50,7 @@ public class Shooter extends SubsystemBase {
   public boolean velReached;
   public double m_fireVel;
   public double m_fireAngle;
+  CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
   
   /*
    * Creates a new Shooter subsystem.
@@ -81,6 +84,13 @@ public class Shooter extends SubsystemBase {
     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));
+
+
+
+    m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_hoodUpLimit.enableLimitSwitch(true);
+    m_hoodDownLimit.enableLimitSwitch(true);
   }
 
   @Override
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
index b6ffef2..beeccda 100644
--- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -7,10 +7,12 @@
 
 package frc4388.robot.subsystems;
 
+import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.ControlType;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
@@ -22,6 +24,7 @@ import frc4388.robot.Constants.ShooterConstants;
 public class ShooterAim extends SubsystemBase {
   public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
   public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
+  CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit;
 
     // Configure PID Controllers
     CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
@@ -32,10 +35,15 @@ public class ShooterAim extends SubsystemBase {
   public ShooterAim() {
     resetGyroShooterRotate();
     m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
+
+    m_shooterRightLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_shooterLeftLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_shooterRightLimit.enableLimitSwitch(true);
+    m_shooterLeftLimit.enableLimitSwitch(true);
   }
 
   public void runShooterWithInput(double input) {
-    m_shooterRotateMotor.set(input);
+    m_shooterRotateMotor.set(input/3);
   }
 
 
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index 74b2484..ec258c5 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -20,12 +20,13 @@ 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.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.StorageConstants;
 
 public class Storage extends SubsystemBase {
-  private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
+  public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
   private DigitalInput[] m_beamSensors = new DigitalInput[6];
 
   CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
@@ -69,6 +70,11 @@ public class Storage extends SubsystemBase {
     m_encoder.setPosition(0);
   }
 
+  public void testBeams(){
+    SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
+    SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
+  }
+
   /* Storage PID Control */
   public void runStoragePositionPID(double targetPos){
     // Set PID Coefficients
@@ -79,6 +85,8 @@ public class Storage extends SubsystemBase {
     m_storagePIDController.setFF(storageGains.m_kF);
     m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
 
+    SmartDashboard.putNumber("Storage Position PID Target", targetPos);
+    SmartDashboard.putNumber("Storage Position Pos", getEncoderPos());
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
 

From 1dbceadb947c0c09ca9e04896f212ee9638fc184 Mon Sep 17 00:00:00 2001
From: Elijah Price <elijah@theprices.net>
Date: Fri, 28 Feb 2020 20:08:47 -0700
Subject: [PATCH 37/69] Fixed not working storage PID

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

diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
index 9536e24..da8708e 100644
--- a/src/main/java/frc4388/robot/commands/StoragePositionPID.java
+++ b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
@@ -25,14 +25,14 @@ public class StoragePositionPID extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    startPos = m_storage.getEncoderPos();
+
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     System.err.println("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooiujgxzxfghjkiujsdasdgioiedsdjkl");
-    m_storage.runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL);
+    m_storage.runStoragePositionPID(StorageConstants.STORAGE_FULL_BALL);
   }
 
   // Called once the command ends or is interrupted.

From a70983a4c2215dc1879218b8b1fd9944f97bf633 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Fri, 28 Feb 2020 20:48:22 -0700
Subject: [PATCH 38/69] Removed Output Limits, Added GotoCoordinates Command

---
 src/main/java/frc4388/robot/Constants.java    |  2 +-
 .../java/frc4388/robot/RobotContainer.java    |  5 +-
 .../robot/commands/DriveWithJoystick.java     | 25 ++++----
 .../robot/commands/GotoCoordinates.java       | 61 +++++++++++++++++++
 .../java/frc4388/robot/subsystems/Drive.java  |  7 +++
 5 files changed, 86 insertions(+), 14 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/GotoCoordinates.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 5054477..dc283e4 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -40,7 +40,7 @@ public final class Constants {
         public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor
         public static final double NEUTRAL_DEADBAND = 0.04;
         public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
-            new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01);
+            new SupplyCurrentLimitConfiguration(false, 60, 40, 2);
         public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops
 
         /* Drive Train Characteristics */
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 20518cc..65f8912 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -33,6 +33,7 @@ import frc4388.robot.commands.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.DriveStraightToPositionMM;
 import frc4388.robot.commands.DriveStraightToPositionPID;
 import frc4388.robot.commands.DriveWithJoystick;
+import frc4388.robot.commands.GotoCoordinates;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
 import frc4388.robot.commands.RunIntakeWithTriggers;
@@ -116,7 +117,7 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, -192));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
@@ -124,7 +125,7 @@ public class RobotContainer {
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new DrivePositionMPAux(m_robotDrive, 12.0, 3.0, 1, 36.0));
+            .whenPressed(new GotoCoordinates(m_robotDrive, 12, 40));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index 1802f22..54bc129 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -60,19 +60,22 @@ public class DriveWithJoystick extends CommandBase {
     }
     double tempOutputLimit = 0.8;
 
-    if (moveOutput > tempOutputLimit) {
-      moveOutput = tempOutputLimit;
-    } else if(moveOutput < -tempOutputLimit) {
-      moveOutput = -tempOutputLimit;
-    }
+    boolean isOutputLimited = false;
 
-    if (steerOutput > tempOutputLimit) {
-      steerOutput = tempOutputLimit;
-    } else if(steerOutput < -tempOutputLimit) {
-      steerOutput = -tempOutputLimit;
-    }
+    if (isOutputLimited) {
+      if (moveOutput > tempOutputLimit) {
+        moveOutput = tempOutputLimit;
+      } else if(moveOutput < -tempOutputLimit) {
+        moveOutput = -tempOutputLimit;
+      }
 
-    SmartDashboard.putNumber("Steer Output Test", moveOutput);
+      if (steerOutput > tempOutputLimit) {
+        steerOutput = tempOutputLimit;
+      } else if(steerOutput < -tempOutputLimit) {
+        steerOutput = -tempOutputLimit;
+      }
+    }
+    
     m_drive.driveWithInput(moveOutput, steerOutput);
   }
 
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
new file mode 100644
index 0000000..e836820
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class GotoCoordinates extends SequentialCommandGroup {
+  Drive m_drive;
+
+  double m_xTarget;
+  double m_yTarget;
+  double m_currentAngle;
+  double m_hypotDist;
+
+  double m_lastAngle;
+
+  /**
+   * Creates a new GotoPosition.
+   */
+  public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+
+    m_xTarget = xTarget;
+    m_yTarget = yTarget;
+
+    m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
+
+    m_currentAngle = calcAngle();
+
+
+    addCommands(new TurnDegrees(m_drive, m_currentAngle), new DriveStraightToPositionMM(m_drive, m_hypotDist));
+  }
+
+  public boolean isQuadrantThree() {
+    if ((m_xTarget < 0) && (m_yTarget < 0)) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  public double calcAngle() {
+    if (isQuadrantThree()) {
+      return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
+    } else {
+      return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
+    }
+  }
+
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index f4541dc..3380fd0 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -88,6 +88,7 @@ public class Drive extends SubsystemBase {
   public double m_lastAngleYaw = 0;
   public double m_currentAngleYaw = 0;
 
+  public double m_lastAngleGotoCoordinates;
   /* Smart Dashboard Objects */
   SendableChooser<String> m_songChooser = new SendableChooser<String>();
   
@@ -726,6 +727,12 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
       //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
 
+      double leftRPM = m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity();
+      double rightRPM = m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity();
+
+      SmartDashboard.putNumber("Left Motor RPM", leftRPM);
+      SmartDashboard.putNumber("Right Motor RPM", rightRPM);
+
       //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());

From f8c2235ea7a4a5645739fdc3d8af2b30d937d390 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 28 Feb 2020 21:05:40 -0700
Subject: [PATCH 39/69] Testing for storage

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index dbb29f6..08277bc 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -194,6 +194,10 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
             .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.2)))
             .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
+
+        new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
+            .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(7)))
+            .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
     }
 
     /**
diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
index 9536e24..a06b5e7 100644
--- a/src/main/java/frc4388/robot/commands/StoragePositionPID.java
+++ b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
@@ -31,8 +31,7 @@ public class StoragePositionPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.err.println("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooiujgxzxfghjkiujsdasdgioiedsdjkl");
-    m_storage.runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL);
+    m_storage.runStoragePositionPID(StorageConstants.STORAGE_FULL_BALL);
   }
 
   // Called once the command ends or is interrupted.

From 237eaeb7efcaced6e48cd3478f3c0a29fdca9a17 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Fri, 28 Feb 2020 21:25:09 -0700
Subject: [PATCH 40/69] GotoCoordinate Works, Trying to Implement TurnAngle

---
 src/main/java/frc4388/robot/RobotContainer.java        | 10 +++++-----
 .../java/frc4388/robot/commands/GotoCoordinates.java   | 10 ++++++----
 src/main/java/frc4388/robot/subsystems/Drive.java      |  4 ++--
 3 files changed, 13 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 65f8912..9b3b15d 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -125,7 +125,7 @@ public class RobotContainer {
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new GotoCoordinates(m_robotDrive, 12, 40));
+            .whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
@@ -192,10 +192,10 @@ public class RobotContainer {
        
         //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
         //This assumes that we are positioned against the right wall with our shooter facing the target.
-        return new SequentialCommandGroup(new Wait(2, m_robotDrive), 
+        return new SequentialCommandGroup(new Wait(0, m_robotDrive), 
                                     //add aim command
                                     //add shooter command
-                                    new DriveStraightToPositionMM(m_robotDrive, 48.0),
+                                    //new DriveStraightToPositionMM(m_robotDrive, 48.0),
                                     /*new ParallelCommandGroup(
                                         new StorageIntakeGroup(m_robotIntake, m_robotStorage),
                                             new DriveStraightToPositionMM(m_robotDrive, 36.0)),
@@ -207,8 +207,8 @@ public class RobotContainer {
                                     //add shooter command
 //Below this would be the picking up additional balls outside of those in the trench directly behind us
 
-                                    new TurnDegrees(m_robotDrive, -90),
-                                    new DriveStraightToPositionMM(m_robotDrive, 96.0));//,
+                                    //new GotoCoordinates(m_robotDrive, 36, 36),
+                                    new GotoCoordinates(m_robotDrive, 36, 36, 0));//,
                                     //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
                                     //new TurnDegrees(m_robotDrive, 75),
                                     //new DriveStraightToPositionMM(m_robotDrive, 18.0),
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
index e836820..5ca4c75 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -20,13 +20,12 @@ public class GotoCoordinates extends SequentialCommandGroup {
   double m_yTarget;
   double m_currentAngle;
   double m_hypotDist;
-
-  double m_lastAngle;
+  double m_endAngle;
 
   /**
    * Creates a new GotoPosition.
    */
-  public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) {
+  public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
     m_drive = subsystem;
@@ -37,9 +36,12 @@ public class GotoCoordinates extends SequentialCommandGroup {
     m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
 
     m_currentAngle = calcAngle();
+    m_endAngle = endAngle;
 
 
-    addCommands(new TurnDegrees(m_drive, m_currentAngle), new DriveStraightToPositionMM(m_drive, m_hypotDist));
+    addCommands(  new TurnDegrees(m_drive, m_currentAngle), 
+                  new DriveStraightToPositionMM(m_drive, m_hypotDist), 
+                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle - 90));
   }
 
   public boolean isQuadrantThree() {
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 3380fd0..8c2e2bd 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -727,8 +727,8 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
       //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
 
-      double leftRPM = m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity();
-      double rightRPM = m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity();
+      double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+      double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
 
       SmartDashboard.putNumber("Left Motor RPM", leftRPM);
       SmartDashboard.putNumber("Right Motor RPM", rightRPM);

From 8704cf515f2271bb11fe87a90253d8a67dcff17d Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Fri, 28 Feb 2020 21:54:46 -0700
Subject: [PATCH 41/69] Added endAngle to GotoCoordinates

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 9b3b15d..81f4033 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -208,7 +208,7 @@ public class RobotContainer {
 //Below this would be the picking up additional balls outside of those in the trench directly behind us
 
                                     //new GotoCoordinates(m_robotDrive, 36, 36),
-                                    new GotoCoordinates(m_robotDrive, 36, 36, 0));//,
+                                    new GotoCoordinates(m_robotDrive, 36, 36, -90));//,
                                     //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
                                     //new TurnDegrees(m_robotDrive, 75),
                                     //new DriveStraightToPositionMM(m_robotDrive, 18.0),
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
index 5ca4c75..5580cfc 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc4388.robot.subsystems.Drive;
 
@@ -36,12 +37,15 @@ public class GotoCoordinates extends SequentialCommandGroup {
     m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
 
     m_currentAngle = calcAngle();
+
+    SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle); 
+
     m_endAngle = endAngle;
 
 
     addCommands(  new TurnDegrees(m_drive, m_currentAngle), 
                   new DriveStraightToPositionMM(m_drive, m_hypotDist), 
-                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle - 90));
+                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
   }
 
   public boolean isQuadrantThree() {

From 9f1368016c5d9828ccdc89f6a9d9b67a575f1b5d Mon Sep 17 00:00:00 2001
From: Elijah Price <elijah@theprices.net>
Date: Sat, 29 Feb 2020 09:54:43 -0700
Subject: [PATCH 42/69] testing storage PID

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

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 02b6f7b..152dede 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -159,7 +159,7 @@ public final class Constants {
 
         /* PID Gains */
         public static final double STORAGE_MIN_OUTPUT = -1.0;
-        public static final Gains STORAGE_GAINS = new Gains(1, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains STORAGE_GAINS = new Gains(0.1, 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 08277bc..6077671 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -196,8 +196,7 @@ public class RobotContainer {
             .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
 
         new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(7)))
-            .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
+            .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(7)));
     }
 
     /**
diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
index 2fe2c5b..ec6caad 100644
--- a/src/main/java/frc4388/robot/commands/StoragePositionPID.java
+++ b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
@@ -42,10 +42,10 @@ public class StoragePositionPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
+    /*if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos())
     {
       return true;
-    }
+    }*/
     return false;
   }
 }

From 8eddfff30c5e45155fa6c6003e6fc2163c994ae0 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 29 Feb 2020 09:58:53 -0700
Subject: [PATCH 43/69] Update RobotContainer.java

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 81f4033..2fda40a 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -169,8 +169,8 @@ public class RobotContainer {
             .whileHeld(new TrackTarget(m_robotShooter));
 
         //Prepares storage for intaking
-        new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
-            .whileHeld(new InstantCommand(() -> m_robotIntake.runIntake(0.3), m_robotIntake));
+        new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
+            .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage));
             
         //Runs storage to outtake
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)

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

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

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index dc283e4..3f1d249 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -54,7 +54,7 @@ public final class Constants {
         public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
         public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
         public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55);
-        public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
         public static final int DRIVE_CRUISE_VELOCITY = 25000;
         public static final int DRIVE_ACCELERATION = 21000;
 
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 2fda40a..78bfc4c 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -192,7 +192,7 @@ public class RobotContainer {
        
         //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
         //This assumes that we are positioned against the right wall with our shooter facing the target.
-        return new SequentialCommandGroup(new Wait(0, m_robotDrive), 
+        return new SequentialCommandGroup(new Wait(m_robotDrive, 0), 
                                     //add aim command
                                     //add shooter command
                                     //new DriveStraightToPositionMM(m_robotDrive, 48.0),
@@ -208,7 +208,9 @@ public class RobotContainer {
 //Below this would be the picking up additional balls outside of those in the trench directly behind us
 
                                     //new GotoCoordinates(m_robotDrive, 36, 36),
-                                    new GotoCoordinates(m_robotDrive, 36, 36, -90));//,
+                                    //new DriveStraightToPositionPID(m_robotDrive, 160)
+                                    //new DriveStraightToPositionMM(m_robotDrive, 160)
+                                    new GotoCoordinates(m_robotDrive, 87, 47, -90));//,
                                     //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
                                     //new TurnDegrees(m_robotDrive, 75),
                                     //new DriveStraightToPositionMM(m_robotDrive, 18.0),
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
index 5580cfc..d267cbe 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -43,8 +43,9 @@ public class GotoCoordinates extends SequentialCommandGroup {
     m_endAngle = endAngle;
 
 
-    addCommands(  new TurnDegrees(m_drive, m_currentAngle), 
-                  new DriveStraightToPositionMM(m_drive, m_hypotDist), 
+    addCommands(  new TurnDegrees(m_drive, m_currentAngle),
+                  new Wait(m_drive, 1),
+                  new DriveStraightToPositionPID(m_drive, m_hypotDist), 
                   new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
   }
 
diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java
index 934b508..ee7f63f 100644
--- a/src/main/java/frc4388/robot/commands/Wait.java
+++ b/src/main/java/frc4388/robot/commands/Wait.java
@@ -21,7 +21,7 @@ public class Wait extends CommandBase {
   /**
    * Creates a new WaitCommand.
    */
-  public Wait(float seconds, SubsystemBase subsystem) {
+  public Wait(SubsystemBase subsystem, float seconds) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_waitTime = (long) (seconds * 1000);

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

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

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

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

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

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

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

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

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 3f1d249..fd1e83d 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -54,7 +54,7 @@ public final class Constants {
         public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
         public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
         public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55);
-        public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.05, 0, 1.0);
         public static final int DRIVE_CRUISE_VELOCITY = 25000;
         public static final int DRIVE_ACCELERATION = 21000;
 
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index a00158e..9737315 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -117,7 +117,7 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 60));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 240));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)

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

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

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

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

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

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

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

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

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

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

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index e57c495..9900e90 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -28,6 +28,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
+import frc4388.robot.commands.AutoPath1FromCenter;
+import frc4388.robot.commands.AutoPath2FromRight;
 import frc4388.robot.commands.DrivePositionMPAux;
 import frc4388.robot.commands.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.DriveStraightToPositionMM;
@@ -117,7 +119,7 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 240));
+            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 24));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
@@ -125,7 +127,7 @@ public class RobotContainer {
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new Wait(m_robotDrive, 0));
+            .whenPressed(new Wait(m_robotDrive, 0, 0));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
@@ -190,23 +192,9 @@ public class RobotContainer {
         // Run path following command, then stop at the end.
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
        
-        //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
-        //This assumes that we are positioned against parallel to the initiation line with our back bumper lined up with the center target
-        return new SequentialCommandGroup(new Wait(m_robotDrive, 0), 
-                                    //shoot pre-loaded 3 balls
-                                    new GotoCoordinates(m_robotDrive, 75, 44, -90),
-                                    //Start Intake Ball 1
-                                    new GotoCoordinates(m_robotDrive, 0, 12, 0),
-                                    new GotoCoordinates(m_robotDrive, 0, 28, 0),
-                                    //Start Intake Ball 2
-                                    new GotoCoordinates(m_robotDrive, 0, 8, 0),
-                                    new GotoCoordinates(m_robotDrive, 0, 28, 0),
-                                     //Start Intake Ball 3
-                                     new GotoCoordinates(m_robotDrive, 0, 8, 0)
-                                     /*Shoot 3 Balls*/ );
+        //return new AutoPath1FromCenter(m_robotDrive);
+        return new AutoPath2FromRight(m_robotDrive);
 
-                                    /*new GotoCoordinates(m_robotDrive, 0, 68.75, 0),*/
-                                    //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
     }
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(
diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
new file mode 100644
index 0000000..ced8a88
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class AutoPath1FromCenter extends SequentialCommandGroup {
+  Drive m_drive;
+
+  /**
+   * Creates a new AutoPath1FromCenter.
+   */
+  public AutoPath1FromCenter(Drive subsystem) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+
+    addCommands(  new Wait(m_drive, 0, 1), 
+                  //shoot pre-loaded 3 balls
+                  new GotoCoordinates(m_drive, 75, 44, -90),
+                  //Start Intake Ball 1
+                  new GotoCoordinates(m_drive, 0, 12, 0),
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 2
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 3
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new Wait(m_drive, 0, 2)
+                  //Shoot 3 Balls
+                  );
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
new file mode 100644
index 0000000..e1ebaa0
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class AutoPath2FromRight extends SequentialCommandGroup {
+  Drive m_drive;
+
+  /**
+   * Creates a new AutoPath2FromRight.
+   */
+  public AutoPath2FromRight(Drive subsystem) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+    
+    addCommands(  new Wait(m_drive, 0, 1),
+                  new GotoCoordinates(m_drive, 0, 77, 0),
+                  //Start Intake Ball 1
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 2
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  //Shoot 5 Balls
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 1 (second round)
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new Wait(m_drive, 0, 2)
+                  );
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
index 9c3fbcf..cfa8d2e 100644
--- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java
+++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java
@@ -44,7 +44,7 @@ public class GotoCoordinates extends SequentialCommandGroup {
 
 
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
-                  new Wait(m_drive, 0.1),
+                  new Wait(m_drive, 0, 0),
                   new DriveStraightToPositionPID(m_drive, m_hypotDist), 
                   new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
   }
diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java
index 4288abb..580d279 100644
--- a/src/main/java/frc4388/robot/commands/Wait.java
+++ b/src/main/java/frc4388/robot/commands/Wait.java
@@ -17,15 +17,19 @@ public class Wait extends CommandBase {
   long m_waitTime;
   long m_currentTime;
   SubsystemBase m_subsystem;
+  int m_waitNum;
+
+  int counter  = 0;
 
   /**
    * Creates a new WaitCommand.
    */
-  public Wait(SubsystemBase subsystem, double seconds) {
+  public Wait(SubsystemBase subsystem, double seconds, int waitNum) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_waitTime = (long) (seconds * 1000);
     m_subsystem = subsystem;
+    m_waitNum = waitNum;
 
     addRequirements(m_subsystem);
   }
@@ -40,8 +44,15 @@ public class Wait extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+
+    if (counter == 0) {
+      SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
+    }
+
     m_currentTime = System.currentTimeMillis();
     SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
+
+    counter ++;
   }
 
   // Called once the command ends or is interrupted.

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

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

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

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

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

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

From 493c4f69de00c27da7b91d685481a851433cac55 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 29 Feb 2020 18:42:37 -0700
Subject: [PATCH 54/69] hood turret and tracking working

---
 src/main/java/frc4388/robot/Constants.java    |  6 +-
 .../java/frc4388/robot/RobotContainer.java    | 27 +++++---
 .../robot/commands/CalibrateShooter.java      | 63 +++++++++++++++++++
 .../frc4388/robot/commands/HoldTarget.java    | 20 ++++--
 .../robot/commands/HoodPositionPID.java       | 48 ++++++++++++++
 .../frc4388/robot/commands/TrackTarget.java   |  8 +--
 .../frc4388/robot/subsystems/Shooter.java     | 23 ++++---
 .../frc4388/robot/subsystems/ShooterAim.java  | 24 ++++---
 8 files changed, 184 insertions(+), 35 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/CalibrateShooter.java
 create mode 100644 src/main/java/frc4388/robot/commands/HoodPositionPID.java

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 02b6f7b..52fa895 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -122,7 +122,7 @@ public final class Constants {
         //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0);
         public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 0, 1.0);
         public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
-        public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
+        public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 1.0);
         public static final double SHOOTER_TURRET_MIN = -1.0;
         public static final double ENCODER_TICKS_PER_REV = 2048;
         public static final double NEO_UNITS_PER_REV = 42;
@@ -169,8 +169,8 @@ public final class Constants {
     
     public static final class VisionConstants {
         public static final double FOV = 29.8; //Field of view of limelight
-        public static final double TARGET_HEIGHT = 82.75;
-        public static final double LIME_ANGLE = 18.7366;
+        public static final double TARGET_HEIGHT = 64;
+        public static final double LIME_ANGLE = 25;
         public static final double TURN_P_VALUE = 0.65;
         public static final double X_ANGLE_ERROR = 1.3;
         public static final double MOTOR_DEAD_ZONE = 0.3;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 08277bc..46f2b90 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -16,22 +16,26 @@ import edu.wpi.first.wpilibj.controller.RamseteController;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
+import frc4388.robot.commands.CalibrateShooter;
 import frc4388.robot.commands.DrivePositionMPAux;
 import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.DriveStraightToPositionMM;
 import frc4388.robot.commands.DriveStraightToPositionPID;
 import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
 import frc4388.robot.commands.HoldTarget;
+import frc4388.robot.commands.HoodPositionPID;
 import frc4388.robot.commands.DriveWithJoystickDriveStraight;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
@@ -169,14 +173,18 @@ public class RobotContainer {
 
         // safety for climber and leveler
         new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
-            .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
-            .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
+            //.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
+            //.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
+            .whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Angle Current", m_robotShooter.m_angleAdjustMotor.getOutputCurrent())))
+            .whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Rotate Current", m_robotShooterAim.m_shooterRotateMotor.getOutputCurrent())));
 
         // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
-            //.whileHeld(new TrackTarget(m_robotShooter,m_robotShooterAim))
-            .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
+            //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
+            .whileHeld(new ParallelCommandGroup(
+                new HoldTarget(m_robotShooter,m_robotShooterAim),
+                new HoodPositionPID(m_robotShooter)));
+            //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
             //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
 
         //Prepares storage for intaking
@@ -188,16 +196,17 @@ public class RobotContainer {
             .whileHeld(new StorageOutake(m_robotStorage));
 
         new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.2)))
+            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3)))
             .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
 
         new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.2)))
+            .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3)))
             .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
 
         new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
-            .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(7)))
-            .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
+            .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim));
+            
+        
     }
 
     /**
diff --git a/src/main/java/frc4388/robot/commands/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/CalibrateShooter.java
new file mode 100644
index 0000000..1349f1c
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/CalibrateShooter.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* 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 com.revrobotics.CANSparkMax.SoftLimitDirection;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+
+public class CalibrateShooter extends CommandBase {
+  Shooter m_shooter;
+  ShooterAim m_shooterAim;
+  /**
+   * Creates a new CalibrateShooter.
+   */
+  public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) {
+    m_shooter = shootSub;
+    m_shooterAim = aimSub;
+    addRequirements(m_shooter, m_shooterAim);
+  }
+
+  // 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_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
+    m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
+    m_shooter.m_angleEncoder.setPosition(0);
+    m_shooter.m_angleAdjustMotor.set(-0.1);
+
+    m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
+    m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
+    m_shooterAim.m_shooterRotateEncoder.setPosition(0);
+    m_shooterAim.m_shooterRotateMotor.set(0.075);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
+    m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+
+    m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
+    m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java
index bbbe656..791266d 100644
--- a/src/main/java/frc4388/robot/commands/HoldTarget.java
+++ b/src/main/java/frc4388/robot/commands/HoldTarget.java
@@ -41,7 +41,7 @@ public class HoldTarget extends CommandBase {
   public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
     m_shooterAim = aimSubsystem;
     m_shooter = shooterSubsystem;
-    addRequirements(m_shooter);
+    addRequirements(m_shooterAim);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
   }
 
@@ -69,7 +69,7 @@ public class HoldTarget extends CommandBase {
         //Deadzones
       else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
       else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
-      m_shooterAim.runShooterWithInput(turnAmount/3);
+      m_shooterAim.runShooterWithInput(-turnAmount);
 
         //Finding Distance
       distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
@@ -79,17 +79,29 @@ public class HoldTarget extends CommandBase {
       double xVel = (distance*VisionConstants.GRAV)/(yVel);
 
       fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
-      fireAngle = Math.atan(yVel/xVel);
+      fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
       m_shooter.m_fireVel = fireVel;
       m_shooter.m_fireAngle = fireAngle;
 
+    }/*
+    else{
+      System.err.println("Shooter Pos: " + m_shooterAim.getShooterRotatePosition());
+      double curveInput = -Math.abs(-Math.cos(Math.PI * ((2*m_shooterAim.getShooterRotatePosition())/55))+1) * 0.1;
+      if (m_shooterAim.getShooterRotatePosition() >= -3 || m_shooterAim.getShooterRotatePosition() <= -54){
+        curveInput = -curveInput;
+      }
+      System.err.println("Curve Input: " + curveInput);
+      
+      m_shooterAim.runShooterWithInput(curveInput);
     }
+    */
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java
new file mode 100644
index 0000000..dd276f0
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/HoodPositionPID.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* 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.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Shooter;
+
+public class HoodPositionPID extends CommandBase {
+  Shooter m_shooter;
+  /**
+   * Creates a new HoodPositionPID.
+   */
+  public HoodPositionPID(Shooter subSystem) {
+    m_shooter = subSystem;
+    addRequirements(m_shooter);
+  }
+
+  // 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() {
+    double firingAngle = (-0.47*m_shooter.addFireAngle())+40.5;
+    SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
+    SmartDashboard.putNumber("Fire Angle", firingAngle);
+    m_shooter.runAngleAdjustPID(firingAngle);
+  }
+
+  // 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/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 5db2669..2e0e512 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -74,7 +74,7 @@ public class TrackTarget extends CommandBase {
       } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
         turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
       }
-      m_shooterAim.runShooterWithInput(turnAmount);
+      m_shooterAim.runShooterWithInput(-turnAmount);
 
       // Finding Distance
       distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
@@ -84,17 +84,17 @@ public class TrackTarget extends CommandBase {
       double xVel = (distance * VisionConstants.GRAV) / (yVel);
 
       fireVel = Math.sqrt((Math.pow(xVel, 2)) + (Math.pow(yVel, 2)));
-      fireAngle = Math.atan(yVel / xVel);
+      fireAngle = Math.atan(yVel / xVel) * (180/Math.PI);
       m_shooter.m_fireVel = fireVel;
       m_shooter.m_fireAngle = fireAngle;
-
     }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index c8bb07d..1f0a699 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -13,6 +13,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
 import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
@@ -35,7 +36,7 @@ public class Shooter extends SubsystemBase {
   public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
 
   CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
-  CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
+  public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
 
   public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
   public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
@@ -57,10 +58,10 @@ public class Shooter extends SubsystemBase {
    */
   public Shooter() {
     //Testing purposes reseting gyros
-    resetGyroAngleAdj();
+    //resetGyroAngleAdj();
     m_shooterFalcon.configFactoryDefault();
     m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
-    m_shooterFalcon.setInverted(false);
+    m_shooterFalcon.setInverted(true);
     m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
     m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
     setShooterGains();
@@ -91,6 +92,11 @@ public class Shooter extends SubsystemBase {
     m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
     m_hoodUpLimit.enableLimitSwitch(true);
     m_hoodDownLimit.enableLimitSwitch(true);
+
+    m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
+    m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, 33);
+    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, 3);
   }
 
   @Override
@@ -106,6 +112,7 @@ public class Shooter extends SubsystemBase {
     return m_fireAngle;
   }
 
+
   /**
    * Runs drum shooter motor.
    * @param speed Speed to set the motor at
@@ -126,9 +133,8 @@ public class Shooter extends SubsystemBase {
   }
 
   /* Angle Adjustment PID Control */
-  public void runAngleAdjustPID(double mmtargetAngle)
+  public void runAngleAdjustPID(double targetAngle)
   {
-    double targetAngle = addFireAngle();
     // Set PID Coefficients
     m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
     m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
@@ -137,9 +143,6 @@ public class Shooter extends SubsystemBase {
     m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
     m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); 
 
-    // Convert input angle in degrees to rotations of the motor
-    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
-
     m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
   }
 
@@ -162,4 +165,8 @@ public class Shooter extends SubsystemBase {
   public void resetGyroAngleAdj(){
       m_angleEncoder.setPosition(0);
   }
+
+  public double getAnglePosition(){
+    return m_angleAdjustMotor.getEncoder().getPosition();
+  }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
index beeccda..be69e18 100644
--- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -14,9 +14,11 @@ import com.revrobotics.CANSparkMax;
 import com.revrobotics.ControlType;
 import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
 import frc4388.robot.Constants.ShooterConstants;
@@ -28,18 +30,24 @@ public class ShooterAim extends SubsystemBase {
 
     // Configure PID Controllers
     CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
-    CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
+    public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
+
   /**
    * Creates a new ShooterAim.
    */
   public ShooterAim() {
-    resetGyroShooterRotate();
+    //resetGyroShooterRotate();
     m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
 
-    m_shooterRightLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
-    m_shooterLeftLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
     m_shooterRightLimit.enableLimitSwitch(true);
     m_shooterLeftLimit.enableLimitSwitch(true);
+
+    m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
+    m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
+    m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, -2);
+    m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, -56);
   }
 
   public void runShooterWithInput(double input) {
@@ -64,11 +72,13 @@ public class ShooterAim extends SubsystemBase {
     m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
   }
 
-  
-    /* For Testing Purposes, reseting gyro for shooter rotation */
     public void resetGyroShooterRotate()
     {
-      m_shooterRotateEncoder.setPosition(0);
+        m_shooterRotateEncoder.setPosition(0);
+    }
+
+    public double getShooterRotatePosition(){
+      return m_shooterRotateMotor.getEncoder().getPosition();
     }
 
   @Override

From eb129c6b342a35b843344c1491a340d45a039067 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 29 Feb 2020 21:03:19 -0700
Subject: [PATCH 55/69] Full sequence possibly working, Full turret working

---
 src/main/java/frc4388/robot/RobotContainer.java     | 13 +++++++------
 .../frc4388/robot/commands/HoodPositionPID.java     |  9 +++++++--
 .../java/frc4388/robot/commands/ShootFireGroup.java |  2 +-
 .../java/frc4388/robot/commands/ShootPrepGroup.java |  3 ++-
 .../frc4388/robot/commands/StoragePrepIntake.java   |  5 ++---
 .../java/frc4388/robot/commands/StorageRun.java     |  2 ++
 .../java/frc4388/robot/commands/TrackTarget.java    |  7 +++----
 src/main/java/frc4388/utility/ShooterTables.java    |  4 ++--
 8 files changed, 26 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index d49e95d..7cb314e 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -62,6 +62,7 @@ import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.commands.StorageOutake;
 import frc4388.robot.commands.StoragePrepAim;
 import frc4388.robot.commands.StoragePrepIntake;
+import frc4388.robot.commands.StorageRun;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.LimeLight;
@@ -180,12 +181,12 @@ public class RobotContainer {
 
         // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
-            .whileHeld(new ParallelCommandGroup(
-                new HoldTarget(m_robotShooter,m_robotShooterAim),
-                new HoodPositionPID(m_robotShooter)));
-            //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
-            //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
+            .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
+            //.whileHeld(new ParallelCommandGroup(
+                //new HoldTarget(m_robotShooter,m_robotShooterAim),
+                //new HoodPositionPID(m_robotShooter)))
+            .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
+            
 
         //Prepares storage for intaking
         new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS)
diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java
index dd276f0..d713f8b 100644
--- a/src/main/java/frc4388/robot/commands/HoodPositionPID.java
+++ b/src/main/java/frc4388/robot/commands/HoodPositionPID.java
@@ -13,12 +13,13 @@ import frc4388.robot.subsystems.Shooter;
 
 public class HoodPositionPID extends CommandBase {
   Shooter m_shooter;
+  double firingAngle;
   /**
    * Creates a new HoodPositionPID.
    */
   public HoodPositionPID(Shooter subSystem) {
     m_shooter = subSystem;
-    addRequirements(m_shooter);
+    //addRequirements(m_shooter);
   }
 
   // Called when the command is initially scheduled.
@@ -29,7 +30,7 @@ public class HoodPositionPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double firingAngle = (-0.47*m_shooter.addFireAngle())+40.5;
+    firingAngle = (-0.47*m_shooter.addFireAngle())+40.5;
     SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
     SmartDashboard.putNumber("Fire Angle", firingAngle);
     m_shooter.runAngleAdjustPID(firingAngle);
@@ -43,6 +44,10 @@ public class HoodPositionPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
+    double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
+    if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
+      return true;
+    }
     return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
index 10eb073..7d23c65 100644
--- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java
@@ -28,7 +28,7 @@ public class ShootFireGroup extends ParallelRaceGroup {
       new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
       new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
       new HoldTarget(m_shooter, m_shooterAim),
-      new StoragePositionPID(m_storage)
+      new StorageRun(m_storage)
     );
   }
 }
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
index 7407365..0615b7d 100644
--- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
@@ -27,7 +27,8 @@ public class ShootPrepGroup extends ParallelCommandGroup {
     addCommands(
       new TrackTarget(m_shooter, m_shooterAim),
       new ShooterVelocityControlPID(m_shooter),
-      new StoragePrepAim(m_storage)
+      new StoragePrepAim(m_storage),
+      new HoodPositionPID(m_shooter)
     );
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
index 643c858..6a9f36e 100644
--- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
+++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
@@ -36,8 +36,7 @@ public class StoragePrepIntake extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.err.println(m_storage.getBeam(0));
-    if (m_storage.getBeam(0)){//&& System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed
+    if (m_storage.getBeam(0)){
       m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
     }
     else{
@@ -53,7 +52,7 @@ public class StoragePrepIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(0)){
+    if (!m_storage.getBeam(0) || startTime + 2000 <= System.currentTimeMillis()){
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/StorageRun.java b/src/main/java/frc4388/robot/commands/StorageRun.java
index 5b56b52..b97d226 100644
--- a/src/main/java/frc4388/robot/commands/StorageRun.java
+++ b/src/main/java/frc4388/robot/commands/StorageRun.java
@@ -29,12 +29,14 @@ public class StorageRun extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.err.println("oiudhgisbjkljasbhfkofhdnsekdfjbsjfvsdkcfbsdjhfgvsdkjfbsd");
     m_storage.runStorage(StorageConstants.STORAGE_SPEED);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    m_storage.runStorage(0);
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 2e0e512..fc49d81 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -83,8 +83,8 @@ public class TrackTarget extends CommandBase {
       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) * (180/Math.PI);
+      fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
+      fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
       m_shooter.m_fireVel = fireVel;
       m_shooter.m_fireAngle = fireAngle;
     }
@@ -93,8 +93,7 @@ public class TrackTarget extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java
index 638f140..dc7fa83 100644
--- a/src/main/java/frc4388/utility/ShooterTables.java
+++ b/src/main/java/frc4388/utility/ShooterTables.java
@@ -93,7 +93,7 @@ public class ShooterTables {
         SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]);
     }
 
-    public double getHood(double distance) {
+    public double getHood(double distance) { //Rotations of motor
         int i = 0;
         while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
             i ++;
@@ -108,7 +108,7 @@ public class ShooterTables {
         }
     }
 
-    public double getVelocity(double distance) {
+    public double getVelocity(double distance) { //Units per 100ms
         int i = 0;
         while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
             i ++;

From 272951ed67ad1ee168e5433f78d0a1e29bb66621 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 29 Feb 2020 23:45:07 -0700
Subject: [PATCH 56/69] Gettign trimming to work (this may not work)

---
 .../java/frc4388/robot/RobotContainer.java    | 21 +++---
 src/main/java/frc4388/robot/Trims.java        | 18 +++++
 .../frc4388/robot/commands/HoldTarget.java    | 11 +--
 .../frc4388/robot/commands/TrackTarget.java   | 11 ++-
 .../frc4388/robot/commands/TrimShooter.java   | 68 +++++++++++++++++++
 .../frc4388/robot/subsystems/Shooter.java     |  6 ++
 .../frc4388/robot/subsystems/ShooterAim.java  |  1 +
 7 files changed, 122 insertions(+), 14 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/Trims.java
 create mode 100644 src/main/java/frc4388/robot/commands/TrimShooter.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 7cb314e..674daa8 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -59,6 +59,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.TrimShooter;
 import frc4388.robot.commands.StorageOutake;
 import frc4388.robot.commands.StoragePrepAim;
 import frc4388.robot.commands.StoragePrepIntake;
@@ -97,6 +98,7 @@ public class RobotContainer {
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
 
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -174,17 +176,12 @@ public class RobotContainer {
 
         // safety for climber and leveler
         new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
-            //.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
-            //.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
-            .whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Angle Current", m_robotShooter.m_angleAdjustMotor.getOutputCurrent())))
-            .whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Rotate Current", m_robotShooterAim.m_shooterRotateMotor.getOutputCurrent())));
+            .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
+            .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
 
         // starts tracking target
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
-            //.whileHeld(new ParallelCommandGroup(
-                //new HoldTarget(m_robotShooter,m_robotShooterAim),
-                //new HoodPositionPID(m_robotShooter)))
             .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
             
 
@@ -196,16 +193,24 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
             .whileHeld(new StorageOutake(m_robotStorage));
 
+        //TEST FOR HOOD
         new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
             .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3)))
             .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
 
+        //TEST FOR HOOD
         new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
             .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3)))
             .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
 
+        //Trims shooter
+        new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS)
+            .whenPressed(new TrimShooter(m_robotShooter));
+
+        //Calibrates turret and hood
         new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
             .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim));
+
     }
 
     /**
@@ -323,4 +328,4 @@ public class RobotContainer {
     {
         return m_driverXbox.getJoyStick();
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Trims.java b/src/main/java/frc4388/robot/Trims.java
new file mode 100644
index 0000000..859794f
--- /dev/null
+++ b/src/main/java/frc4388/robot/Trims.java
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* 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.robot;
+
+public class Trims{
+    public double m_turretTrim;
+    public double m_hoodTrim;
+
+    public Trims(double turretTrim, double hoodTrim){
+        m_turretTrim = turretTrim;
+        m_hoodTrim = hoodTrim;
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java
index 791266d..cfed837 100644
--- a/src/main/java/frc4388/robot/commands/HoldTarget.java
+++ b/src/main/java/frc4388/robot/commands/HoldTarget.java
@@ -32,8 +32,11 @@ public class HoldTarget extends CommandBase {
   double yAngle = 0;
   double target = 0;
   public double distance;
-  public static double fireVel;
-  public static double fireAngle;
+  public double fireVel;
+  public double fireAngle;
+
+  public double m_hoodTrim;
+  public double m_turretTrim;
 
   /**
    * Uses the Limelight to track the target
@@ -69,7 +72,7 @@ public class HoldTarget extends CommandBase {
         //Deadzones
       else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
       else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
-      m_shooterAim.runShooterWithInput(-turnAmount);
+      m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
 
         //Finding Distance
       distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
@@ -81,7 +84,7 @@ public class HoldTarget extends CommandBase {
       fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
       fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
       m_shooter.m_fireVel = fireVel;
-      m_shooter.m_fireAngle = fireAngle;
+      m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
 
     }/*
     else{
diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index fc49d81..95689e8 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -14,12 +14,14 @@ import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.commands.TrimShooter;
 import frc4388.utility.controller.IHandController;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class TrackTarget extends CommandBase {
@@ -37,6 +39,9 @@ public class TrackTarget extends CommandBase {
   public static double fireVel;
   public static double fireAngle;
 
+  public double m_hoodTrim;
+  public double m_turretTrim;
+
   /**
    * Uses the Limelight to track the target
    */
@@ -74,7 +79,7 @@ public class TrackTarget extends CommandBase {
       } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
         turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
       }
-      m_shooterAim.runShooterWithInput(-turnAmount);
+      m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
 
       // Finding Distance
       distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
@@ -85,8 +90,10 @@ public class TrackTarget extends CommandBase {
 
       fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
       fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
+
+      
       m_shooter.m_fireVel = fireVel;
-      m_shooter.m_fireAngle = fireAngle;
+      m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
     }
   }
 
diff --git a/src/main/java/frc4388/robot/commands/TrimShooter.java b/src/main/java/frc4388/robot/commands/TrimShooter.java
new file mode 100644
index 0000000..e83d00a
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/TrimShooter.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Joystick;
+import frc4388.utility.controller.XboxController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.Trims;
+import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+
+public class TrimShooter extends CommandBase {
+  private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+  public double turretTrim = 0;
+  public double hoodTrim = 0;
+
+  public Shooter m_shooter;
+  /**
+   * Creates a new TrimShooter.
+   */
+  public TrimShooter(Shooter shootSub) {
+    m_shooter = shootSub;
+  }
+
+  // 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_operatorXbox.getDPadTop()){
+      hoodTrim += 1;
+    }
+    else if(m_operatorXbox.getDPadBottom()){
+      hoodTrim -= 1;
+    }
+    else if(m_operatorXbox.getDPadRight()){
+      turretTrim += 1;
+    }
+    else if(m_operatorXbox.getDPadLeft()){
+      turretTrim -= 1;
+    }
+
+    m_shooter.shooterTrims.m_turretTrim = turretTrim;
+    m_shooter.shooterTrims.m_hoodTrim = hoodTrim;
+  }
+
+  // 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/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index 1f0a699..b46247a 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
+import frc4388.robot.Trims;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.utility.ShooterTables;
 import frc4388.utility.controller.IHandController;
@@ -52,6 +53,8 @@ public class Shooter extends SubsystemBase {
   public double m_fireVel;
   public double m_fireAngle;
   CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
+
+  public Trims shooterTrims;
   
   /*
    * Creates a new Shooter subsystem.
@@ -59,6 +62,8 @@ public class Shooter extends SubsystemBase {
   public Shooter() {
     //Testing purposes reseting gyros
     //resetGyroAngleAdj();
+    shooterTrims = new Trims(0, 0);
+
     m_shooterFalcon.configFactoryDefault();
     m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
     m_shooterFalcon.setInverted(true);
@@ -97,6 +102,7 @@ public class Shooter extends SubsystemBase {
     m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
     m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, 33);
     m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, 3);
+    
   }
 
   @Override
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
index be69e18..fde5350 100644
--- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -32,6 +32,7 @@ public class ShooterAim extends SubsystemBase {
     CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
     public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
 
+
   /**
    * Creates a new ShooterAim.
    */

From d11083e560b57160fc79106dfd57cffab6945ca3 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sun, 1 Mar 2020 00:41:23 -0700
Subject: [PATCH 57/69] Conventions

No functional change
---
 src/main/java/frc4388/robot/Constants.java        | 15 +++++++++++++++
 .../frc4388/robot/commands/CalibrateShooter.java  |  9 ++++++---
 .../java/frc4388/robot/commands/HoldTarget.java   |  2 ++
 .../frc4388/robot/commands/HoodPositionPID.java   |  5 ++++-
 .../frc4388/robot/commands/ShootPrepGroup.java    |  2 +-
 .../robot/commands/ShooterVelocityControlPID.java | 10 +++++-----
 .../frc4388/robot/commands/StorageIntake.java     |  4 +++-
 .../frc4388/robot/commands/StorageOutake.java     |  3 ++-
 .../robot/commands/StoragePositionPID.java        |  3 ++-
 .../frc4388/robot/commands/StoragePrepAim.java    |  7 +++++--
 .../frc4388/robot/commands/StoragePrepIntake.java |  6 ++++--
 .../java/frc4388/robot/commands/StorageRun.java   |  1 -
 .../java/frc4388/robot/commands/TrackTarget.java  |  2 ++
 .../java/frc4388/robot/commands/TrimShooter.java  |  3 ++-
 .../java/frc4388/robot/subsystems/Shooter.java    |  8 ++++----
 .../java/frc4388/robot/subsystems/ShooterAim.java |  8 ++++----
 .../java/frc4388/robot/subsystems/Storage.java    | 11 -----------
 17 files changed, 61 insertions(+), 38 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index a567edb..3408679 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -127,6 +127,20 @@ public final class Constants {
         public static final double ENCODER_TICKS_PER_REV = 2048;
         public static final double NEO_UNITS_PER_REV = 42;
         public static final double DEGREES_PER_ROT = 360;
+
+        public static final int TURRET_RIGHT_SOFT_LIMIT = -2;
+        public static final int TURRET_LEFT_SOFT_LIMIT = -55;
+        public static final double TURRET_SPEED_MULTIPLIER = 0.3;
+        public static final double TURRET_CALIBRATE_SPEED = 0.075;
+
+        public static final int HOOD_UP_SOFT_LIMIT = 33;
+        public static final int HOOD_DOWN_SOFT_LIMIT = 3;
+        public static final double HOOD_CONVERT_SLOPE = 0.47;
+        public static final double HOOD_CONVERT_B = 40.5;
+        public static final double HOOD_CALIBRATE_SPEED = 0.1;
+
+        public static final double DRUM_RAMP_LIMIT = 1000;
+        public static final double DRUM_VELOCITY_BOUND = 300;
     }
     
     public static final class ClimberConstants {
@@ -142,6 +156,7 @@ public final class Constants {
         public static final double STORAGE_PARTIAL_BALL = 2;
         public static final double STORAGE_FULL_BALL = 7;
         public static final double STORAGE_SPEED = 0.5;
+        public static final double STORAGE_TIMEOUT = 2000;
        
         /* Ball Indexes */
         public static final int BEAM_SENSOR_DIO_0 = 0;
diff --git a/src/main/java/frc4388/robot/commands/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/CalibrateShooter.java
index 1349f1c..1b766d7 100644
--- a/src/main/java/frc4388/robot/commands/CalibrateShooter.java
+++ b/src/main/java/frc4388/robot/commands/CalibrateShooter.java
@@ -11,6 +11,7 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 
@@ -18,7 +19,9 @@ public class CalibrateShooter extends CommandBase {
   Shooter m_shooter;
   ShooterAim m_shooterAim;
   /**
-   * Creates a new CalibrateShooter.
+   * Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders
+   * @param shootSub The Shooter subsystem
+   * @param aimSub The ShooterAim subsystem
    */
   public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) {
     m_shooter = shootSub;
@@ -37,12 +40,12 @@ public class CalibrateShooter extends CommandBase {
     m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
     m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
     m_shooter.m_angleEncoder.setPosition(0);
-    m_shooter.m_angleAdjustMotor.set(-0.1);
+    m_shooter.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
 
     m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
     m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
     m_shooterAim.m_shooterRotateEncoder.setPosition(0);
-    m_shooterAim.m_shooterRotateMotor.set(0.075);
+    m_shooterAim.m_shooterRotateMotor.set(ShooterConstants.TURRET_CALIBRATE_SPEED);
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java
index cfed837..cbdee25 100644
--- a/src/main/java/frc4388/robot/commands/HoldTarget.java
+++ b/src/main/java/frc4388/robot/commands/HoldTarget.java
@@ -40,6 +40,8 @@ public class HoldTarget extends CommandBase {
 
   /**
    * Uses the Limelight to track the target
+   * @param shooterSubsystem The Shooter subsystem
+   * @param aimSubsystem The ShooterAim subsystem
    */
   public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
     m_shooterAim = aimSubsystem;
diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java
index d713f8b..efecb58 100644
--- a/src/main/java/frc4388/robot/commands/HoodPositionPID.java
+++ b/src/main/java/frc4388/robot/commands/HoodPositionPID.java
@@ -9,6 +9,7 @@ package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.subsystems.Shooter;
 
 public class HoodPositionPID extends CommandBase {
@@ -30,7 +31,9 @@ public class HoodPositionPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    firingAngle = (-0.47*m_shooter.addFireAngle())+40.5;
+    double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
+    double b = ShooterConstants.HOOD_CONVERT_B;
+    firingAngle = (-slope*m_shooter.addFireAngle())+b;
     SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
     SmartDashboard.putNumber("Fire Angle", firingAngle);
     m_shooter.runAngleAdjustPID(firingAngle);
diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
index 0615b7d..5c20683 100644
--- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
+++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java
@@ -18,7 +18,7 @@ import frc4388.robot.subsystems.Storage;
 // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
 public class ShootPrepGroup extends ParallelCommandGroup {
   /**
-   * Preps the Shooter to be fired
+   * Prepares the Shooter to be fired
    * @param m_shooter The Shooter subsytem
    * @param m_shooterAim The ShooterAim subsystem
    * @param m_storage The Storage subsytem
diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index a5e0b5a..52dea7f 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -9,6 +9,7 @@ package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.subsystems.Shooter;
 
 public class ShooterVelocityControlPID extends CommandBase {
@@ -16,9 +17,8 @@ public class ShooterVelocityControlPID extends CommandBase {
   double m_targetVel;
   double m_actualVel;
   /**
-   * Creates a new ShooterVelocityControlPID.
+   * Runs the drum at a velocity
    * @param subsystem The Shooter subsytem
-   * @param targetVel The target velocity
    */
   public ShooterVelocityControlPID(Shooter subsystem) {
     m_shooter = subsystem;
@@ -48,9 +48,9 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    //Tells wether the target velocity has been reached
-    double upperBound = m_targetVel + 300;
-    double lowerBound = m_targetVel - 300;
+    //Tells whether the target velocity has been reached
+    double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND;
+    double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND;
     if (m_actualVel < upperBound && m_actualVel > lowerBound){
       SmartDashboard.putBoolean("ShooterVelocityPID Finished", true);
       return true;
diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/StorageIntake.java
index 69de480..baf1630 100644
--- a/src/main/java/frc4388/robot/commands/StorageIntake.java
+++ b/src/main/java/frc4388/robot/commands/StorageIntake.java
@@ -18,7 +18,9 @@ public class StorageIntake extends CommandBase {
   public Storage m_storage;
   public boolean intook;
   /**
-   * Creates a new storageIntake.
+   * Runs the Storage in for intaking
+   * @param inSub The Intake subsystem
+   * @param storeSub The Storage subsytem
    */
   public StorageIntake(Intake inSub, Storage storeSub) {
     m_intake = inSub;
diff --git a/src/main/java/frc4388/robot/commands/StorageOutake.java b/src/main/java/frc4388/robot/commands/StorageOutake.java
index c19ce5a..3231600 100644
--- a/src/main/java/frc4388/robot/commands/StorageOutake.java
+++ b/src/main/java/frc4388/robot/commands/StorageOutake.java
@@ -14,7 +14,8 @@ import frc4388.robot.subsystems.Storage;
 public class StorageOutake extends CommandBase {
   Storage m_storage;
   /**
-   * Creates a new storageOutake.
+   * Runs the Storage out for outaking
+   * @param storeSub The Storage subsystem
    */
   public StorageOutake(Storage storeSub) {
     m_storage = storeSub;
diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
index ec6caad..bc607e5 100644
--- a/src/main/java/frc4388/robot/commands/StoragePositionPID.java
+++ b/src/main/java/frc4388/robot/commands/StoragePositionPID.java
@@ -15,7 +15,8 @@ public class StoragePositionPID extends CommandBase {
   public Storage m_storage;
   double startPos;
   /**
-   * Creates a new StoragePositionPID.
+   * Moves the storage a number of rotations
+   * @param subsystem The Storage subsystem
    */
   public StoragePositionPID(Storage subsystem) {
     m_storage = subsystem;
diff --git a/src/main/java/frc4388/robot/commands/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/StoragePrepAim.java
index 2782c7c..f0e52c1 100644
--- a/src/main/java/frc4388/robot/commands/StoragePrepAim.java
+++ b/src/main/java/frc4388/robot/commands/StoragePrepAim.java
@@ -14,8 +14,10 @@ import frc4388.robot.subsystems.Storage;
 
 public class StoragePrepAim extends CommandBase {
   Storage m_storage;
+  double startTime;
   /**
-   * Creates a new storagePrepAim.
+   * Prepares the Storage for aiming
+   * @param storeSub The Storage subsystem
    */
   public StoragePrepAim(Storage storeSub) {
     m_storage = storeSub;
@@ -25,6 +27,7 @@ public class StoragePrepAim extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    startTime = System.currentTimeMillis();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -46,7 +49,7 @@ public class StoragePrepAim extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(1)){
+    if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
       SmartDashboard.putBoolean("StoragePrepAim Finished", true);
       return true;
     }
diff --git a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
index 6a9f36e..44a2d2e 100644
--- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
+++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java
@@ -18,7 +18,9 @@ public class StoragePrepIntake extends CommandBase {
   public double startTime;
 
   /**
-   * Creates a new StoragePrepIntake.
+   * Prepares the Storage for intaking
+   * @param inSub The Intake subsystem
+   * @param storeSub the Storage subsystem
    */
   public StoragePrepIntake(Intake inSub, Storage storeSub) {
     m_intake = inSub;
@@ -52,7 +54,7 @@ public class StoragePrepIntake extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (!m_storage.getBeam(0) || startTime + 2000 <= System.currentTimeMillis()){
+    if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){
       return true;
     }
     return false;
diff --git a/src/main/java/frc4388/robot/commands/StorageRun.java b/src/main/java/frc4388/robot/commands/StorageRun.java
index b97d226..91632f5 100644
--- a/src/main/java/frc4388/robot/commands/StorageRun.java
+++ b/src/main/java/frc4388/robot/commands/StorageRun.java
@@ -29,7 +29,6 @@ public class StorageRun extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.err.println("oiudhgisbjkljasbhfkofhdnsekdfjbsjfvsdkcfbsdjhfgvsdkjfbsd");
     m_storage.runStorage(StorageConstants.STORAGE_SPEED);
   }
 
diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 95689e8..44295d2 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -44,6 +44,8 @@ public class TrackTarget extends CommandBase {
 
   /**
    * Uses the Limelight to track the target
+   * @param shooterSubsystem The Shooter subsystem
+   * @param aimSubsystem The ShooterAim subsystem
    */
   public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
     m_shooterAim = aimSubsystem;
diff --git a/src/main/java/frc4388/robot/commands/TrimShooter.java b/src/main/java/frc4388/robot/commands/TrimShooter.java
index e83d00a..b77a4ba 100644
--- a/src/main/java/frc4388/robot/commands/TrimShooter.java
+++ b/src/main/java/frc4388/robot/commands/TrimShooter.java
@@ -23,7 +23,8 @@ public class TrimShooter extends CommandBase {
 
   public Shooter m_shooter;
   /**
-   * Creates a new TrimShooter.
+   * Trims the shooter based on the D-Pad inputs
+   * @param shootSub The Shooter subsytems
    */
   public TrimShooter(Shooter shootSub) {
     m_shooter = shootSub;
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index b46247a..cd0dda3 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -57,7 +57,7 @@ public class Shooter extends SubsystemBase {
   public Trims shooterTrims;
   
   /*
-   * Creates a new Shooter subsystem.
+   * Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter.
    */
   public Shooter() {
     //Testing purposes reseting gyros
@@ -100,8 +100,8 @@ public class Shooter extends SubsystemBase {
 
     m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
     m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
-    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, 33);
-    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, 3);
+    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
+    m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
     
   }
 
@@ -159,7 +159,7 @@ public class Shooter extends SubsystemBase {
    */
   public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
     SmartDashboard.putNumber("shoot", actualVel);
-    if (actualVel < targetVel - 1000){
+    if (actualVel < targetVel - ShooterConstants.DRUM_RAMP_LIMIT){
       m_shooterFalcon.set(TalonFXControlMode.PercentOutput, 1);//Ramp up
     }
     else{ //PID Based on targetVel
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
index fde5350..1111749 100644
--- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -34,7 +34,7 @@ public class ShooterAim extends SubsystemBase {
 
 
   /**
-   * Creates a new ShooterAim.
+   * Creates a subsytem for the turret aiming
    */
   public ShooterAim() {
     //resetGyroShooterRotate();
@@ -47,12 +47,12 @@ public class ShooterAim extends SubsystemBase {
 
     m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
     m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
-    m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, -2);
-    m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, -56);
+    m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT);
+    m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT);
   }
 
   public void runShooterWithInput(double input) {
-    m_shooterRotateMotor.set(input/3);
+    m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER);
   }
 
 
diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java
index ec258c5..7bcc411 100644
--- a/src/main/java/frc4388/robot/subsystems/Storage.java
+++ b/src/main/java/frc4388/robot/subsystems/Storage.java
@@ -102,15 +102,4 @@ public class Storage extends SubsystemBase {
   public void setStoragePID(double position){
     m_storagePIDController.setReference(position , ControlType.kPosition);
   }
-
-
-  /*
-  If pressing aim
-    run unti; hitting top beam
-  else
-    Run until hitting bottom beam
-    dont run intake if balls not at bottom
-  
-  2 beamms total
-  */
 }

From 5cd8f25cca44e085e81d6fe434248b5d80d330a0 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sun, 1 Mar 2020 00:51:13 -0700
Subject: [PATCH 58/69] Start of CSV integration

No angle displacement
---
 src/main/java/frc4388/robot/commands/TrackTarget.java | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 44295d2..8bdae59 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -15,6 +15,7 @@ import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.commands.TrimShooter;
+import frc4388.utility.ShooterTables;
 import frc4388.utility.controller.IHandController;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -42,6 +43,8 @@ public class TrackTarget extends CommandBase {
   public double m_hoodTrim;
   public double m_turretTrim;
 
+  ShooterTables m_shooterTable;
+
   /**
    * Uses the Limelight to track the target
    * @param shooterSubsystem The Shooter subsystem
@@ -60,6 +63,7 @@ public class TrackTarget extends CommandBase {
     // Vision Processing Mode
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+    m_shooterTable = new ShooterTables();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -87,11 +91,18 @@ public class TrackTarget extends CommandBase {
       distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
       SmartDashboard.putNumber("Distance to Target", distance);
 
+        //START Equation Code
       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) * (180/Math.PI);
+        //END Equation Code
+
+        /*//START CSV Code
+      fireVel = m_shooterTable.getVelocity(distance);
+      fireAngle = m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different
+        //END CSV Code*/
 
       
       m_shooter.m_fireVel = fireVel;

From 2aed3ffa1d106419de91d2c759b983a76fd46975 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sun, 1 Mar 2020 11:24:57 -0700
Subject: [PATCH 59/69] CAN ids maatch Robots

---
 src/main/java/frc4388/robot/Constants.java | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index dc283e4..f0782cb 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -110,9 +110,9 @@ public final class Constants {
   
     public static final class ShooterConstants {
         /* Motor IDs */
-        public static final int SHOOTER_FALCON_ID = -1;
-        public static final int SHOOTER_ANGLE_ADJUST_ID = -1;
-        public static final int SHOOTER_ROTATE_ID = 10;
+        public static final int SHOOTER_FALCON_ID = 8;
+        public static final int SHOOTER_ANGLE_ADJUST_ID = 10;
+        public static final int SHOOTER_ROTATE_ID = 9;
 
         /* PID Constants Shooter */
         public static final int SHOOTER_SLOT_IDX = 0;
@@ -127,15 +127,15 @@ public final class Constants {
     }
     
     public static final class ClimberConstants {
-        public static final int CLIMBER_SPARK_ID = 10;
+        public static final int CLIMBER_SPARK_ID = 14;
     }
   
     public static final class LevelerConstants {
-        public static final int LEVELER_CAN_ID = -1;
+        public static final int LEVELER_CAN_ID = 15;
     }
   
     public static final class StorageConstants {
-        public static final int STORAGE_CAN_ID = -1;
+        public static final int STORAGE_CAN_ID = 11;
 
         /* Ball Indexes */
         public static final int BEAM_SENSOR_DIO_0 = 0;

From 7adb1edb0c323d686ae342ee27c2c075ece51a01 Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sun, 1 Mar 2020 12:21:03 -0700
Subject: [PATCH 60/69] Add Limit Switches

Note: Shooter turret and hood limits are set up on the add-shooter branch. Those are more specific and much better tuned.
These currently have no soft limits, and are untested
---
 src/main/java/frc4388/robot/subsystems/Climber.java | 8 ++++----
 src/main/java/frc4388/robot/subsystems/Intake.java  | 6 +++---
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java
index f6c9339..e9adf86 100644
--- a/src/main/java/frc4388/robot/subsystems/Climber.java
+++ b/src/main/java/frc4388/robot/subsystems/Climber.java
@@ -31,10 +31,10 @@ public class Climber extends SubsystemBase {
     m_climberMotor.setIdleMode(IdleMode.kBrake);
     m_climberMotor.setInverted(false);
     
-    m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_climberForwardLimit.enableLimitSwitch(false);
-    m_climberReverseLimit.enableLimitSwitch(false);
+    m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_climberForwardLimit.enableLimitSwitch(true);
+    m_climberReverseLimit.enableLimitSwitch(true);
   }
 
   @Override
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index 6b716f4..f074b33 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -41,9 +41,9 @@ public class Intake extends SubsystemBase {
     m_extenderMotor.setInverted(false);
     
     m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_extenderForwardLimit.enableLimitSwitch(false);
-    m_extenderReverseLimit.enableLimitSwitch(false);
+    m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_extenderForwardLimit.enableLimitSwitch(true);
+    m_extenderReverseLimit.enableLimitSwitch(true);
   }
 
   @Override

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

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

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

From f72c20337b353475a61dfd3cfb57f8cb1d72858f Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sun, 1 Mar 2020 12:39:37 -0700
Subject: [PATCH 62/69] Update RobotContainer.java

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

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 501aab2..718abcb 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -74,7 +74,7 @@ import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.commands.TurnDegrees;
 import frc4388.robot.commands.Wait;
-import frc4388.robot.commands.storageOutake;
+import frc4388.robot.commands.StorageOutake;
 import frc4388.robot.subsystems.Camera;
 import frc4388.robot.subsystems.Leveler;
 import frc4388.robot.subsystems.Pneumatics;

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

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

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

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

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

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

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

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

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

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

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

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

From 913f9b8a9e9537df4ced260175e133c63a51b9c6 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sun, 1 Mar 2020 15:13:00 -0700
Subject: [PATCH 67/69] Work on dead assist

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

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
index 5a68a9f..fd2cbe2 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
@@ -82,7 +82,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     }
 
     /* If move stick is being used */
-    if (moveInput != 0) {
+    if (true) {
       m_deadTimeMove = m_currTime;
       m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
                   + (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());
@@ -122,14 +122,14 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   }
 
   private void runDriveWithInput(double move, double steerInput) {
-    double cosMultiplier = .55;
+    double cosMultiplier = .70;
     double steerOutput = 0;
-    double deadzone = .2;
+    double deadzone = .1;
     /* Curves the steer output to be similarily gradual */
     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);
     }
 
     m_drive.driveWithInput(move, steerOutput);
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index edcbac5..059d3c6 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -114,6 +114,11 @@ public class Drive extends SubsystemBase {
     m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
     m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
 
+    m_rightFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.configClosedloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
+
     /* Config Supply Current Limit (Use only for debugging) */
     //m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
     //m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);

From e0966aa65b90f33b14752f79d98245fac6e36de5 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sun, 1 Mar 2020 15:34:40 -0700
Subject: [PATCH 68/69] Implements Drive Straight

---
 .../java/frc4388/robot/RobotContainer.java    |  2 +-
 .../DriveWithJoystickDriveStraight.java       | 19 +++++++++++++------
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 4793fc2..b488752 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -127,7 +127,7 @@ public class RobotContainer {
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
         
-        m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController()));
+        m_robotDrive.setDefaultCommand(new DriveWithJoystickDriveStraight(m_robotDrive, getDriverController()));
         //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController()));
         
         // drives intake with input from triggers on the opperator controller
diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
index a298f9c..ef9c1b9 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
@@ -21,6 +21,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
   long m_deadTimeSteer, m_deadTimeMove;
   long m_deadTimeout = 100;
   IHandController m_controller;
+  boolean m_isInterrupted;
 
   /**
    * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
@@ -54,6 +55,11 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
     m_deltaTime = System.currentTimeMillis() - m_currTime;
     m_currTime = System.currentTimeMillis();
 
+    if (m_isInterrupted) {
+      resetGyroTarget();
+      m_isInterrupted = false;
+    }
+
     /* If steer stick is being used */
     if (steerInput != 0) {
       m_deadTimeSteer = m_currTime;
@@ -78,14 +84,14 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
   }
 
   private void runDriveWithInput(double move, double steer) {
-    double cosMultiplier = .45;
+    double cosMultiplier = .7;
     double steerOutput = 0;
-    double deadzone = .2;
+    double deadzone = .1;
     /* Curves the steer output to be similarily gradual */
-    if (steer > 0){
-      steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
-    } else {
-      steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
+    if (steer > 0) {
+      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier;
+    } else if (steer < 0) {
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier;
     }
     m_drive.driveWithInput(move, steerOutput);
     System.out.println("Driving With Input");
@@ -108,6 +114,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    m_isInterrupted = interrupted;
   }
 
   // Returns true when the command should end.

From 60b67a89ddee10d8cbe07f49b391913052e8f18d Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Mon, 2 Mar 2020 23:37:45 +0000
Subject: [PATCH 69/69] Remove true thing and replace with good thing

---
 .../robot/commands/DriveWithJoystickUsingDeadAssistPID.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
index fd2cbe2..0b154e7 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
@@ -82,7 +82,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     }
 
     /* If move stick is being used */
-    if (true) {
+    if (moveInput != 0) {
       m_deadTimeMove = m_currTime;
       m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
                   + (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());