extender intake group command DONE

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

@@ -146,6 +146,12 @@ public final class Constants {
     public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration(
         false, 15, 0, 0);
   }
+
+  public static final class ExtenderConstants {
+    public static final double EXTENDER_FORWARD_LIMIT = 250.0;
+    public static final double EXTENDER_REVERSE_LIMIT = 0.0;
+  }
+
   public static final class StorageConstants {
     public static final int STORAGE_CAN_ID = 18;
     public static final int BEAM_SENSOR_SHOOTER = 28; //TODO

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

@@ -54,6 +54,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.NotifierCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -61,6 +62,7 @@ import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
+import frc4388.robot.commands.ExtenderIntakeGroup;
 import frc4388.robot.commands.RunMiddleSwitch;
 import frc4388.robot.commands.Shoot;
 import frc4388.robot.commands.TrackTarget;
@@ -237,8 +239,8 @@ public class RobotContainer {
     // new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
     //     .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret));
   
-    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
-        .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
+    // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+    //     .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
 
     // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
     //     .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
@@ -260,9 +262,9 @@ public class RobotContainer {
     //     .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55)))
     //     .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
       
-    new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft
-        .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000)))
-        .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
+    // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft
+    //     .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000)))
+    //     .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
         .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage))
@@ -272,14 +274,26 @@ public class RobotContainer {
         .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
         .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
 
+    // new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+    //     .whileHeld(new RunExtender(m_robotIntake, m_robotExtender, 0.5))
+    //     .whenReleased(new RunCommand(() -> RunExtender.changeDirection(), m_robotIntake, m_robotExtender));
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+        .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
+        // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection()));
+
+    // new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+    //     .whenPressed(new ParallelCommandGroup(
+    //                  new RunCommand(() -> m_robotIntake.m_intakeMotor.set(ExtenderIntakeGroup.direction * -0.2), m_robotIntake),
+    //                  new RunCommand(() -> m_robotExtender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 0.5), m_robotExtender)))
+    //     .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection()));
+    
+    // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+    //     .whenPressed(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0)));
+
     new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
         .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
-        .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender))
-        .whenReleased(new RunCommand(() -> m_robotExtender.switchDirection(), m_robotExtender));
-
-    // new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
-    //     .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
-    //     .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
+        .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
         .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
@@ -314,10 +328,15 @@ public class RobotContainer {
     new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
       .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
       .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
+      .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
+
       .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
       .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
+      .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
+
       .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
-      .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood));
+      .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
+      .whenReleased(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0), m_robotExtender));
 
     // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
     //     .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true)))

src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import frc4388.robot.subsystems.Extender;
+import frc4388.robot.subsystems.Intake;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ExtenderIntakeGroup extends ParallelRaceGroup {
+  
+  public static int direction = 1; // assume extender starts retracted completely
+
+  /** Creates a new RunExtenderAndIntake. */
+  public ExtenderIntakeGroup(Intake intake, Extender extender) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    ExtenderIntakeGroup.direction = 1;
+    addCommands(new RunIntakeConditionally(intake), new RunExtender(extender));
+  }
+
+  public static void changeDirection() {
+    ExtenderIntakeGroup.direction *= -1;
+  }
+}

src/main/java/frc4388/robot/commands/RunExtender.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ExtenderConstants;
+import frc4388.robot.subsystems.Extender;
+import frc4388.robot.subsystems.Intake;
+
+public class RunExtender extends CommandBase {
+
+  private Extender extender;
+
+  /** Creates a new RunExtender. */
+  public RunExtender(Extender extender) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.extender = extender;
+
+    addRequirements(this.extender);
+  }
+
+  // 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() {
+    this.extender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 1.0); // TODO: change to 1.0 for actual speed, 0.5 is just for testing
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    ExtenderIntakeGroup.changeDirection();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (Math.abs(this.extender.m_extenderMotor.getEncoder().getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT) < 5) {
+      return true;
+    }
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/RunIntakeConditionally.java

@@ -5,25 +5,19 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Extender;
 import frc4388.robot.subsystems.Intake;
 
-public class RunExtenderOut extends CommandBase {
+public class RunIntakeConditionally extends CommandBase {
 
   private Intake intake;
-  private Extender extender;
-  private int direction;
 
-  /** Creates a new RunExtenderOut. */
-  public RunExtenderOut(Intake intake, Extender extender) {
+  /** Creates a new RunIntakeConditionally. */
+  public RunIntakeConditionally(Intake intake) {
     // Use addRequirements() here to declare subsystem dependencies.
-
+    
     this.intake = intake;
-    this.extender = extender;
 
-    this.direction = 1;
-
-    addRequirements(this.intake, this.extender);
+    addRequirements(this.intake);
   }
 
   // Called when the command is initially scheduled.
@@ -32,7 +26,13 @@ public class RunExtenderOut extends CommandBase {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if (ExtenderIntakeGroup.direction > 0) {
+      this.intake.m_intakeMotor.set(-0.4);
+    } else {
+      this.intake.m_intakeMotor.set(0);
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override

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

@@ -4,20 +4,24 @@
 
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.led.ColorFlowAnimation.Direction;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkMaxLimitSwitch;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.ExtenderConstants;
+import frc4388.utility.DesmosServer;
 
 public class Extender extends SubsystemBase {
   
-  CANSparkMax m_extenderMotor;
+  public CANSparkMax m_extenderMotor;
 
   private SparkMaxLimitSwitch m_inLimit;
   private SparkMaxLimitSwitch m_outLimit;
 
   public boolean toggle;
-  public int direction = 1;
 
   /** Creates a new Extender. */
   public Extender(CANSparkMax extenderMotor) {
@@ -26,13 +30,28 @@ public class Extender extends SubsystemBase {
 
     m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
     m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    m_inLimit.enableLimitSwitch(true);
-    m_outLimit.enableLimitSwitch(true);
+    m_inLimit.enableLimitSwitch(false);
+    m_outLimit.enableLimitSwitch(false);
+
+    m_extenderMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ExtenderConstants.EXTENDER_FORWARD_LIMIT);
+    m_extenderMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ExtenderConstants.EXTENDER_REVERSE_LIMIT);
+    setExtenderSoftLimits(true);
+  }
+
+  /**
+   * Set status of extender motor soft limits.
+   * @param set Boolean to set soft limits to.
+   */
+  public void setExtenderSoftLimits(boolean set) {
+    m_extenderMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
+    m_extenderMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition());
+    DesmosServer.putDouble("ExtenderPosition", m_extenderMotor.getEncoder().getPosition());
   }
 
     /**
@@ -47,12 +66,7 @@ public class Extender extends SubsystemBase {
 
   public void runExtender(double input) {
     // if (!m_serializer.getBeam() && input < 0.) return;
-    if (this.direction > 0) {}
-    m_extenderMotor.set(this.direction * input);
-  }
-
-  public void switchDirection() {
-    this.direction = this.direction * -1;
+    m_extenderMotor.set(input);
   }
 
   public double getCurrent() {

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

@@ -12,11 +12,13 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.commands.ExtenderIntakeGroup;
+
 import com.revrobotics.CANSparkMax;
 
 public class Intake extends SubsystemBase {
 
-  private WPI_TalonFX m_intakeMotor;
+  public WPI_TalonFX m_intakeMotor;
   private Serializer m_serializer;
 
   /** Creates a new Intake. */
@@ -28,6 +30,8 @@ public class Intake extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    SmartDashboard.putNumber("Intake Percent Output", m_intakeMotor.get());
+    SmartDashboard.putNumber("Extender Direction", ExtenderIntakeGroup.direction);
   }
   /**
    * Runs The Intake With Triggers as input