Intake fixes

src/main/java/frc4388/robot/Constants.java

@@ -135,6 +135,7 @@ public final class Constants {
     public static final int STORAGE_CAN_ID = 17;
     public static final int BEAM_SENSOR_SHOOTER = 28; //TODO
     public static final int BEAM_SENSOR_INTAKE = 29; //TODO
+    public static final double STORAGE_SPEED = 0.3;
   }
   public static final class LEDConstants {
     public static final int LED_SPARK_ID = 0;

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

@@ -56,6 +56,7 @@ 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;
@@ -88,14 +89,15 @@ public class RobotContainer {
 
   // 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 Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor);
   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 Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom);
+  
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -131,6 +133,7 @@ public class RobotContainer {
     // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret,
     // m_robotSwerveDrive));
 
+    //Swerve Drive
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
             getDriverController().getLeftX(),
@@ -139,6 +142,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
     /*
@@ -157,9 +174,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)
@@ -174,27 +192,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));
   }
 
   /**

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

@@ -20,15 +20,17 @@ public class Intake extends SubsystemBase {
 
   private WPI_TalonFX m_intakeMotor;
   private CANSparkMax m_extenderMotor;
+  private Serializer m_serializer;
   private SparkMaxLimitSwitch m_inLimit;
   private SparkMaxLimitSwitch m_outLimit;
 
   public boolean toggle;
 
   /** Creates a new Intake. */
-  public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor) {
+  public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor, Serializer serializer) {
     m_intakeMotor = intakeMotor;
     m_extenderMotor = extenderMotor; 
+    m_serializer = serializer;
 
     m_extenderMotor.restoreFactoryDefaults();
 
@@ -59,9 +61,15 @@ public class Intake extends SubsystemBase {
    * @param extended Wether the Extender Is Extended
    */
   public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?)
-    double extenderMotorSpeed = extended ? 0.25d : 0.d;
+    if (!m_serializer.getBeam() && !extended) return;
+    double extenderMotorSpeed = extended ? 0.25d : -0.25d;
     m_extenderMotor.set(extenderMotorSpeed);
   }
+
+  public void runExtender(double input) {
+    if (!m_serializer.getBeam() && input < 0.) return;
+    m_extenderMotor.set(input);
+  }
   /**
    * Toggles The Extender
   */
@@ -69,5 +77,4 @@ public class Intake extends SubsystemBase {
     toggle = !toggle;
     runExtender(toggle);
   }
-  //Test
 }

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

@@ -3,6 +3,7 @@ package frc4388.robot.subsystems;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants;
+import frc4388.robot.Constants.SerializerConstants;
 import edu.wpi.first.wpilibj.DigitalInput;
 import com.revrobotics.CANSparkMax;
 
@@ -14,15 +15,16 @@ public class Serializer extends SubsystemBase{
 
   public Serializer(CANSparkMax serializerBelt, /*CANSparkMax serializerShooterBelt,*/ DigitalInput serializerBeam) { 
     m_serializerBelt = serializerBelt;
-    // m_serializerShooterBelt = serializerShooterBelt;
     m_serializerBeam = serializerBeam;
 
-    serializerState = false;
-    setSerializerState(serializerState);
     m_serializerBelt.set(0);
     // m_serializerShooterBelt.set(0);
     
   }
+
+  public void setSerializer(double input){
+    m_serializerBelt.set(input);
+  }
   /**
    * Gets The State Of The Beam
    * @return The State Of The Beam
@@ -30,40 +32,16 @@ public class Serializer extends SubsystemBase{
   public boolean getBeam() {
     return m_serializerBeam.get();
   }
+
   /**
    * Sets The Serializer State With The Beam
    * @param state Your State Of The Button
    * @param beambroken The State of the Beam Senser
    */
-  public void setSerializerStateWithBeam(boolean state, boolean beambroken) {
-    boolean total = state || beambroken;
-    setSerializerState(total);
+  public void setSerializerStateWithBeam() {
+    if (m_serializerBeam.get()) setSerializer(0.0);
+    else setSerializer(SerializerConstants.SERIALIZER_BELT_SPEED);
   }
-  /**
-   * Sets The Serializer State With The Beam 
-   * @param state Your State Of The Button
-   */
-  public void setSerializerState(boolean state) {
-    setSerializerBeltState(state);
-    // setSerializerShooterBeltState(state);
-    serializerState = state;
-  }
-  /**
-   * Sets the Serializer Belt State
-   * @param state Your State Of The Button
-   */
-  public void setSerializerBeltState(boolean state) {
-    double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d;
-    m_serializerBelt.set(serializerBeltSpeed);
-  }
-  // /**
-  //  * Sets the Shooter Belt State
-  //  * @param state Your State Of The Button
-  //  */
-  // public void setSerializerShooterBeltState(boolean state) {
-  //   double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d;
-  //   m_serializerShooterBelt.set(serializerShooterBeltSpeed);
-  // }
   
   /**
    * Gets the Serializer State

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

@@ -26,9 +26,8 @@ public class Storage extends SubsystemBase {
    * If Else, Stop Running Storage
    */
   public void manageStorage() {
-    if (getBeamIntake()) { //Maybe needs to be shooter
-      runStorage(1.d);
-    } else { runStorage(0.d); }
+    if (getBeamIntake()) runStorage(0.d);
+    else runStorage(1.d);
   }
   
   /**
@@ -43,7 +42,7 @@ public class Storage extends SubsystemBase {
    * @return The State Of The Beam on the Shooter
    */
   public boolean getBeamShooter(){
-    return m_beamShooter.get();
+    return m_beamShooter.get();//True if open
   }
 
   /**
@@ -51,7 +50,7 @@ public class Storage extends SubsystemBase {
    * @return The Beam State Of The Intake
    */
   public boolean getBeamIntake(){
-    return m_beamIntake.get();
+    return m_beamIntake.get(); //True if open
   }