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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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/30] 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 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 19/30] 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 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 20/30] 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 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 21/30] 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 22/30] 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 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 23/30] 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 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 24/30] 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 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 25/30] 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 26/30] 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 27/30] 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 28/30] 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 29/30] 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 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 30/30] 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;