Merge branch 'button-fox' into highlanders-day

src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* 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.shooter;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Leveler;
+import frc4388.robot.subsystems.ShooterHood;
+import frc4388.utility.controller.IHandController;
+
+public class RunHoodWithJoystick extends CommandBase {
+  private ShooterHood m_hood;
+  private IHandController m_controller;
+
+  /**
+   * Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller.
+   * @param subsystem pass the Hood subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
+   * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in
+   * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   */
+  public RunHoodWithJoystick(ShooterHood subsystem, IHandController controller) {
+    m_hood = subsystem;
+    m_controller = controller;
+    addRequirements(m_hood);
+  }
+
+  // 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 input = m_controller.getRightYAxis();
+    m_hood.runHood(input);
+  }
+
+  // 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;
+  }
+}

src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.ShooterHood;
+
+public class ShooterGoalPosition extends CommandBase {
+  Shooter m_shooter;
+  ShooterHood m_hood;
+  ShooterAim m_aim;
+  /**
+   * Creates a new ShooterGoalPosition.
+   */
+  public ShooterGoalPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) {
+    m_shooter = shooterSub;
+    m_hood = hoodSub;
+    m_aim = aimSub;
+    addRequirements(m_shooter,m_hood,m_aim);
+  }
+
+  // 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(5000);
+    m_hood.runAngleAdjustPID(3);
+    m_aim.runshooterRotatePID(-26.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/shooter/ShooterManual.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.shooter;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.RobotContainer;
+
+public class ShooterManual extends CommandBase {
+  public boolean isManual = false;
+  /**
+   * Creates a new ShooterManual.
+   */
+  public ShooterManual(boolean man) {
+    isManual = man;
+  }
+
+  // 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() {
+    RobotContainer.m_isShooterManual = isManual;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.m_isShooterManual = isManual;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.ShooterHood;
+
+public class ShooterTrenchPosition extends CommandBase {
+  Shooter m_shooter;
+  ShooterHood m_hood;
+  ShooterAim m_aim;
+  /**
+   * Creates a new ShooterTrenchPosition.
+   */
+  public ShooterTrenchPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) {
+    m_shooter = shooterSub;
+    m_hood = hoodSub;
+    m_aim = aimSub;
+    addRequirements(m_shooter,m_hood,m_aim);
+  }
+
+  // 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(5000);
+    m_hood.runAngleAdjustPID(3);
+    m_aim.runshooterRotatePID(-26.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/storage/ManageStorage.java

@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
+import frc4388.robot.subsystems.Storage.StorageMode;
 
 public class ManageStorage extends CommandBase {
   Storage m_storage;
@@ -26,17 +27,13 @@ public class ManageStorage extends CommandBase {
 
   /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
   boolean m_isStorageEmpty = true;
-
-  public enum StorageMode{IDLE, INTAKE, RESET};
-  StorageMode m_storageMode = StorageMode.IDLE;
   
   /**
    * Creates a new ManageStorage.
    */
-  public ManageStorage(Storage m_robotStorage, StorageMode storageMode) {
+  public ManageStorage(Storage m_robotStorage) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_storage = m_robotStorage;
-    m_storageMode = storageMode;
     addRequirements(m_storage);
   }
 
@@ -50,7 +47,7 @@ public class ManageStorage extends CommandBase {
 
     m_isStorageEmpty = !m_isBallInStorage;
 
-    if (m_storageMode == StorageMode.RESET) {
+    if (m_storage.m_storageMode == StorageMode.RESET) {
       m_resetStartTime = System.currentTimeMillis();
     }
   }
@@ -68,12 +65,14 @@ public class ManageStorage extends CommandBase {
     SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
     SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
 
-    if (m_storageMode == StorageMode.IDLE) {
+    if (m_storage.m_storageMode == StorageMode.IDLE) {
       runIdle();
-    } else if (m_storageMode == StorageMode.INTAKE) {
+    } else if (m_storage.m_storageMode == StorageMode.INTAKE) {
       runIntake();
-    } else if (m_storageMode == StorageMode.RESET) {
+    } else if (m_storage.m_storageMode == StorageMode.RESET) {
       runReset();
+    } else if (m_storage.m_storageMode == StorageMode.MANUAL) {
+      runManual();
     }
   }
 
@@ -91,10 +90,10 @@ public class ManageStorage extends CommandBase {
       }
       if (m_isStorageEmpty && m_isBallInStorage) { // If Ball moves into storage, set storage to full and swtich to idle mode
         m_isStorageEmpty = false;
-        m_storageMode = StorageMode.IDLE;
+        m_storage.changeStorageMode(StorageMode.IDLE);
       }
     } else {
-      m_storageMode = StorageMode.IDLE;
+      m_storage.changeStorageMode(StorageMode.IDLE);
     }
   }
 
@@ -106,7 +105,7 @@ public class ManageStorage extends CommandBase {
     m_storage.runStorage(0);
 
     if (m_isBallInIntake) {
-      m_storageMode = StorageMode.INTAKE;
+      m_storage.changeStorageMode(StorageMode.INTAKE);
     }
     m_isStorageEmpty = !m_isBallInStorage;
   }
@@ -120,17 +119,24 @@ public class ManageStorage extends CommandBase {
     m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
 
     if (m_isBallInIntake) {
-      m_storageMode = StorageMode.INTAKE;
+      m_storage.changeStorageMode(StorageMode.INTAKE);
     } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
-      m_storageMode = StorageMode.IDLE;
+      m_storage.changeStorageMode(StorageMode.IDLE);
     }
     m_isStorageEmpty = !m_isBallInStorage;
   }
 
+  /**
+   * Switches Storage to Manual only
+   */
+  private void runManual() {
+    m_storage.runStorage(0);
+  }
+
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_storageMode = StorageMode.RESET;
+    m_storage.changeStorageMode(StorageMode.RESET);
   }
 
   // Returns true when the command should end.

src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java

@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.subsystems.Storage;
+import frc4388.robot.subsystems.Storage.StorageMode;
 
 public class ManageStoragePID extends CommandBase {
   Storage m_storage;
@@ -30,16 +31,13 @@ public class ManageStoragePID extends CommandBase {
   /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
   boolean m_isStorageEmpty = true;
 
-  public enum StorageMode{IDLE, INTAKE, RESET};
-  StorageMode m_storageMode = StorageMode.IDLE;
   
   /**
    * Creates a new ManageStorage.
    */
-  public ManageStoragePID(Storage m_robotStorage, StorageMode storageMode) {
+  public ManageStoragePID(Storage m_robotStorage) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_storage = m_robotStorage;
-    m_storageMode = storageMode;
     addRequirements(m_storage);
   }
 
@@ -55,7 +53,7 @@ public class ManageStoragePID extends CommandBase {
 
     m_intakeStartPos = m_storage.getEncoderPosInches();
 
-    if (m_storageMode == StorageMode.RESET) {
+    if (m_storage.m_storageMode == StorageMode.RESET) {
       m_resetStartTime = System.currentTimeMillis();
     }
   }
@@ -73,11 +71,11 @@ public class ManageStoragePID extends CommandBase {
     SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
     SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
 
-    if (m_storageMode == StorageMode.IDLE) {
+    if (m_storage.m_storageMode == StorageMode.IDLE) {
       runIdle();
-    } else if (m_storageMode == StorageMode.INTAKE) {
+    } else if (m_storage.m_storageMode == StorageMode.INTAKE) {
       runIntake();
-    } else if (m_storageMode == StorageMode.RESET) {
+    } else if (m_storage.m_storageMode == StorageMode.RESET) {
       runReset();
     }
   }
@@ -93,10 +91,10 @@ public class ManageStoragePID extends CommandBase {
 
       double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches();
       if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) {
-        m_storageMode = StorageMode.IDLE;
+        m_storage.changeStorageMode(StorageMode.IDLE);
       }
     } else {
-      m_storageMode = StorageMode.IDLE;
+      m_storage.changeStorageMode(StorageMode.IDLE);
     }
   }
 
@@ -108,7 +106,7 @@ public class ManageStoragePID extends CommandBase {
     m_storage.runStorage(0);
 
     if (m_isBallInIntake) {
-      m_storageMode = StorageMode.INTAKE;
+      m_storage.changeStorageMode(StorageMode.INTAKE);
       m_intakeStartPos = m_storage.getEncoderPosInches();
     }
     m_isStorageEmpty = !m_isBallInStorage;
@@ -123,10 +121,10 @@ public class ManageStoragePID extends CommandBase {
     m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
 
     if (m_isBallInIntake) {
-      m_storageMode = StorageMode.INTAKE;
+      m_storage.changeStorageMode(StorageMode.INTAKE);
       m_intakeStartPos = m_storage.getEncoderPosInches();
     } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
-      m_storageMode = StorageMode.IDLE;
+      m_storage.changeStorageMode(StorageMode.IDLE);
     }
     m_isStorageEmpty = !m_isBallInStorage;
   }
@@ -134,7 +132,7 @@ public class ManageStoragePID extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_storageMode = StorageMode.RESET;
+    m_storage.changeStorageMode(StorageMode.RESET);
   }
 
   // Returns true when the command should end.