Merge branch 'full-robot-test' into vision-odo-not-proto

src/main/java/frc4388/robot/RobotContainer.java

@@ -56,12 +56,16 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
 import frc4388.robot.commands.Shoot;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
+import frc4388.robot.subsystems.Intake;
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.Serializer;
+import frc4388.robot.subsystems.Storage;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
 import frc4388.robot.subsystems.Vision;
@@ -84,17 +88,17 @@ public class RobotContainer {
   /* RobotMap */
   private final RobotMap m_robotMap = new RobotMap();
 
-  /* Subsystems */
-  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack,
-      m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
-
+  // Subsystems 
+  public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
+  private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
+  private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer);
+  private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
   private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
   private final Hood m_robotHood = new Hood();
   private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
-
   private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
-  // private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
+  private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom);
 
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -130,6 +134,7 @@ public class RobotContainer {
 
     m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
 
+    //Swerve Drive
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
             getDriverController().getLeftX(),
@@ -138,6 +143,20 @@ public class RobotContainer {
             getDriverController().getRightY(),
             true),
             m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
+    //Intake with Triggers
+    m_robotIntake.setDefaultCommand(
+        new RunCommand(() -> m_robotIntake.runWithTriggers(
+            getOperatorController().getLeftTriggerAxis(), 
+            getOperatorController().getRightTriggerAxis()),
+            m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
+    //Storage Management
+    m_robotStorage.setDefaultCommand(
+        new RunCommand(() -> m_robotStorage.manageStorage(), 
+        m_robotStorage).withName("Storage manageStorage defaultCommand"));
+    //Serializer Management
+    m_robotSerializer.setDefaultCommand(
+        new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), 
+        m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
 
     // continually sends updates to the Blinkin LED controller to keep the lights on
     /*
@@ -156,9 +175,10 @@ public class RobotContainer {
    * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+    
     /* Driver Buttons */
     // "XboxController.Button.kBack" was undefined yet, 7 works just fine
-    new JoystickButton(getDriverController(), 7)
+    new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
         .whenPressed(m_robotSwerveDrive::resetGyro);
 
     new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
@@ -173,27 +193,39 @@ public class RobotContainer {
 
     new JoystickButton(getDriverController(), XboxController.Button.kA.value)
         .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
-    new JoystickButton(getDriverController(), XboxController.Button.kX.value)
+
+    new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp
         .whenPressed(() -> m_robotMap.leftFront.reset())
         .whenPressed(() -> m_robotMap.rightFront.reset())
         .whenPressed(() -> m_robotMap.leftBack.reset())
         .whenPressed(() -> m_robotMap.rightBack.reset());
 
     /* Operator Buttons */
-    // activates "Lit Mode"
     /*
-     * new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-     * .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-     * .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
-     * 
-     * new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
-     * .whenPressed(new InstantCommand());
-     * 
      * // activates "BoomBoom"
      * new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
      * .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret,
      * m_robotHood));
      */
+
+    //Extender
+    new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+        .whenPressed(() -> m_robotIntake.runExtender(true));
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+        .whenPressed(() -> m_robotIntake.runExtender(false));
+
+
+
+    //Storage
+    new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
+        .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))
+        .whenReleased(() -> m_robotStorage.runStorage(0.0));
+
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
+        .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))
+        .whenReleased(() -> m_robotStorage.runStorage(0.0));
   }
 
   /**