Merge branch 'master' into buff-driver-station

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

@@ -17,6 +17,7 @@ import frc4388.utility.LEDPatterns;
 public class LED extends SubsystemBase {
 
   public static float currentLED;
+  public static float defaultLED;
   public static Spark LEDController;
 
   /**
@@ -25,7 +26,8 @@ public class LED extends SubsystemBase {
    */
   public LED(){
     LEDController = new Spark(LEDConstants.LED_SPARK_ID);
-    setPattern(LEDConstants.DEFAULT_PATTERN);
+    defaultLED = LEDConstants.DEFAULT_PATTERN.getValue();
+    runDefaultLED();
     LEDController.set(currentLED);
     System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
   }
@@ -38,6 +40,37 @@ public class LED extends SubsystemBase {
     LEDController.set(currentLED);
   }
 
+  /**
+   * 
+   */
+  public void runDefaultLED() {
+    setPattern(defaultLED);
+  }
+
+  /**
+   * Changes the default LED by an amount
+   * @param amount the amount to increment led by
+   */
+  public void incrementLED(float amount) {
+    defaultLED += amount;
+    if (defaultLED > 1) {
+      defaultLED -= 2;
+    }
+    if (defaultLED < -1) {
+      defaultLED += 2;
+    }
+  }
+
+  /**
+   * Sets the current LED pattern. This method should only be run
+   * whenever you want to change the current LED.
+   * @param pattern LEDPattern to set the Blinkin to.
+   */
+  public void setPattern(float pattern){
+    currentLED = pattern;
+    LEDController.set(pattern);
+  }
+
   /**
    * Sets the current LED pattern. This method should only be run
    * whenever you want to change the current LED.

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

@@ -91,8 +91,11 @@ public class Shooter extends SubsystemBase {
 
       SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
 
-      SmartDashboard.putBoolean("Drum Ready", m_isDrumReady);
-    } catch(Exception e) {
+      SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady);
+    }
+
+    catch(Exception e)
+    {
       
     }
   }

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

@@ -67,6 +67,8 @@ public class ShooterAim extends SubsystemBase {
     SmartDashboard.putBoolean("Aim Ready", m_isAimReady);
 
     SmartDashboard.putData("Turret Angle", m_turretGyro);
+    
+    SmartDashboard.putBoolean("Turret Aimed" , m_isAimReady);
   }
 
   /**

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

@@ -21,7 +21,10 @@ import frc4388.utility.Gains;
 
 public class Storage extends SubsystemBase {
   public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
-  private DigitalInput[] m_beamSensors = new DigitalInput[6];
+  private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
+  private DigitalInput m_beamUseless = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS);
+  private DigitalInput m_beamStorage = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE);
+  private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
 
   CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
 
@@ -38,26 +41,29 @@ public class Storage extends SubsystemBase {
    */
   public Storage() {
     resetEncoder();
-    m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
-    m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS);
-    m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE);
-    m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
+
+    // Set PID Coefficients
+    m_storagePIDController.setP(storageGains.m_kP);
+    m_storagePIDController.setI(storageGains.m_kI);
+    m_storagePIDController.setD(storageGains.m_kD);
+    m_storagePIDController.setIZone(storageGains.m_kIzone);
+    m_storagePIDController.setFF(storageGains.m_kF);
+    m_storagePIDController.setOutputRange(storageGains.m_kminOutput, storageGains.m_kmaxOutput);
   }
 
   @Override
   public void periodic() {
-    //SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
-    //SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
-    //SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get());
-    //SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get());
+    SmartDashboard.putBoolean("Intake Beam", getBeamIntake());
+    SmartDashboard.putBoolean("Storage Beam", getBeamStorage());
+    SmartDashboard.putBoolean("Upper Beam", getBeamUseless());
+    SmartDashboard.putBoolean("Shooter Beam", getBeamShooter());
   }
 
   /**
    * Runs storage motor
    * 
-   * @param input the voltage to run motor at
+   * @param input the percent output to run motor at
    */
-  
   public void runStorage(double input) {
     m_storageMotor.set(input);
   }
@@ -66,36 +72,66 @@ 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 */
+  /**
+   * Runs Storage to a particular position
+   * @param targetPos in inches
+   */
   public void runStoragePositionPID(double targetPos){
-    // Set PID Coefficients
-    m_storagePIDController.setP(storageGains.m_kP);
-    m_storagePIDController.setI(storageGains.m_kI);
-    m_storagePIDController.setD(storageGains.m_kD);
-    m_storagePIDController.setIZone(storageGains.m_kIzone);
-    m_storagePIDController.setFF(storageGains.m_kF);
-    m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput);
-
     //SmartDashboard.putNumber("Storage Position PID Target", targetPos);
     //SmartDashboard.putNumber("Storage Position Pos", getEncoderPos());
+    targetPos = InchesToMotorRots(targetPos);
     m_storagePIDController.setReference(targetPos, ControlType.kPosition);
   }
 
+  /**
+   * Runs Storage to a particular position
+   * @param position in motor rotations
+   */
+  public void setStoragePID(double position){
+    m_storagePIDController.setReference(position, ControlType.kPosition);
+  }
 
   public double getEncoderPos(){
     return m_encoder.getPosition();
   }
 
-  public boolean getBeam(int id){
-    return m_beamSensors[id].get();
+  public double getEncoderPosInches(){
+    return motorRotsToInches(getEncoderPos());
   }
 
-  public void setStoragePID(double position){
-    m_storagePIDController.setReference(position , ControlType.kPosition);
+  public double getEncoderVel(){
+    return m_encoder.getVelocity();
+  }
+
+  /**
+   * @param motorRots
+   * @return inches
+   */
+  public double motorRotsToInches(double motorRots) {
+    return motorRots * (1/StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT) * (StorageConstants.INCHES_PER_STORAGE_ROT);
+  }
+
+  /**
+   * @param inches
+   * @return motorRots
+   */
+  public double InchesToMotorRots(double inches) {
+    return inches * (1/StorageConstants.INCHES_PER_STORAGE_ROT) * (StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT);
+  }
+
+  public boolean getBeamShooter(){
+    return m_beamShooter.get();
+  }
+
+  public boolean getBeamUseless(){
+    return m_beamUseless.get();
+  }
+
+  public boolean getBeamStorage(){
+    return m_beamStorage.get();
+  }
+
+  public boolean getBeamIntake(){
+    return m_beamIntake.get();
   }
 }