Merge branch 'main' into dev

src/main/java/frc/robot/RobotContainer.java

@@ -150,6 +150,7 @@ public RobotContainer() {
             will.rightBumper(),
             will.leftBumper()));
 
+    // FIXME: This error is here to kind of guide you...
     armSubsystem.setDefaultCommand(
         new ArmManualCommand(
             armSubsystem,
@@ -321,13 +322,15 @@ private void configureButtonBindings() {
 
     jasonLayer
         .off(jason.b())
+        // FIXME: This error is here to kind of guide you...
         .onTrue(new ArmPositionCommand(armSubsystem, Arm.Setpoints.SHELF_INTAKE))
         .whileTrue(
             new ForceOuttakeSubsystemModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.INTAKE));
 
-    // reset
+    // Reset arm position
     jasonLayer
         .off(jason.y())
+        // FIXME: This error is here to kind of guide you...
         .onTrue(new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED))
         .onTrue(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED));
     jason.start().onTrue(new SetZeroModeCommand(armSubsystem));
@@ -344,23 +347,19 @@ private void configureButtonBindings() {
                         ? IntakeSubsystem.Modes.INTAKE_LOW
                         : IntakeSubsystem.Modes.INTAKE));
 
-    jason
-        .povUp()
-        .onTrue(
-            new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.OUTTAKE)
-                .alongWith(new ArmPositionCommand(armSubsystem, Arm.Setpoints.HANDOFF)));
-
     jason.start().onTrue(new ZeroIntakeCommand(intakeSubsystem));
 
     jason
         .back()
         .whileTrue(
             new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)
+                // FIXME: This error is here to kind of guide you...
                 .alongWith(new ArmPositionCommand(armSubsystem, Arm.Setpoints.HANDOFF))
                 .alongWith(
                     new ForceOuttakeSubsystemModeCommand(
                         outtakeSubsystem, OuttakeSubsystem.Modes.OFF)))
         .onFalse(
+            // FIXME: This error is here to kind of guide you...
             new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED)
                 .alongWith(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED)));
 
@@ -448,7 +447,6 @@ private void setupAutonomousCommands() {
     final Map<String, Command> eventMap =
         Map.of(
             "stow arm",
-            new ArmPositionCommand(armSubsystem, Constants.Arm.Setpoints.STOWED),
             "zero everything",
             (new SetZeroModeCommand(armSubsystem))
                 .alongWith(new ZeroIntakeCommand(intakeSubsystem)),
@@ -491,6 +489,8 @@ public void end(boolean interrupted) {
                 outtakeSubsystem,
                 armSubsystem,
                 Constants.SCORE_STEP_MAP.get(NodeType.CUBE.atHeight(Height.MID)).subList(0, 1)),
+
+            // FIXME: How can we get this working again?
             "outtake",
             new ScoreCommand(outtakeSubsystem, armSubsystem, drivingCubeOuttake.subList(1, 2), 1)
                 .andThen(

src/main/java/frc/robot/commands/ArmManualCommand.java

@@ -6,19 +6,13 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ArmSubsystem;
-import java.util.function.DoubleSupplier;
 
 public class ArmManualCommand extends CommandBase {
   ArmSubsystem armSubsystem;
-  DoubleSupplier angleSupplier;
-  DoubleSupplier extensionSupplier;
   /** Creates a new AngleArmCommand. */
-  public ArmManualCommand(
-      ArmSubsystem armSubsystem, DoubleSupplier angleSupplier, DoubleSupplier extensionSupplier) {
+  public ArmManualCommand(ArmSubsystem armSubsystem) {
     this.armSubsystem = armSubsystem;
 
-    this.angleSupplier = angleSupplier;
-    this.extensionSupplier = extensionSupplier;
     addRequirements(armSubsystem);
   }
 
@@ -28,15 +22,7 @@ public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-
-    double angleOutput = angleSupplier.getAsDouble();
-    double extensionOutput = extensionSupplier.getAsDouble();
-
-    armSubsystem.setTargetPosition(
-        armSubsystem.getTargetAngleDegrees() + angleOutput,
-        armSubsystem.getTargetExtensionInches() + extensionOutput);
-  }
+  public void execute() {}
 
   // Called once the command ends or is interrupted.
   @Override

src/main/java/frc/robot/commands/ArmPositionCommand.java

@@ -6,30 +6,19 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ArmSubsystem;
-import frc.robot.subsystems.ArmSubsystem.ArmState;
 
 public class ArmPositionCommand extends CommandBase {
   private ArmSubsystem subsystem;
-  private final double desiredAngle;
-  private final double targetExtension;
   /** Creates a new ArmPositionCommand. */
-  public ArmPositionCommand(ArmSubsystem subsystem, double desiredAngle, double targetExtension) {
+  public ArmPositionCommand(ArmSubsystem subsystem) {
     this.subsystem = subsystem;
-    this.desiredAngle = desiredAngle;
-    this.targetExtension = targetExtension;
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(subsystem);
   }
 
-  public ArmPositionCommand(ArmSubsystem subsystem, ArmState armState) {
-    this(subsystem, armState.angle(), armState.extension());
-  }
-
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-    subsystem.setTargetPosition(desiredAngle, targetExtension);
-  }
+  public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -42,6 +31,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return subsystem.atTarget();
+    return false;
   }
 }

src/main/java/frc/robot/commands/GroundPickupCommand.java

@@ -5,8 +5,6 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.Constants.Arm;
-import frc.robot.Constants.Arm.Setpoints;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.OuttakeSubsystem;
@@ -24,20 +22,6 @@ public GroundPickupCommand(
       Supplier<IntakeSubsystem.Modes> modeSupplier) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
-    addCommands(
-        new SetOuttakeModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.INTAKE)
-            .deadlineWith(
-                new IntakeCommand(intakeSubsystem, modeSupplier)
-                    .alongWith(new ArmPositionCommand(armSubsystem, Setpoints.HANDOFF)))
-            .andThen(
-                new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED)
-                    .alongWith(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED))));
-  }
-
-  public GroundPickupCommand(
-      IntakeSubsystem intakeSubsystem,
-      OuttakeSubsystem outtakeSubsystem,
-      ArmSubsystem armSubsystem) {
-    this(intakeSubsystem, outtakeSubsystem, armSubsystem, () -> IntakeSubsystem.Modes.INTAKE);
+    addCommands();
   }
 }

src/main/java/frc/robot/commands/ScoreCommand.java

@@ -70,10 +70,13 @@ private Command createStep(ScoreStep scoreStep) {
     var armState = scoreStep.armState();
     var outtakeState = scoreStep.outtakeState();
     if (armState.isPresent() && outtakeState.isPresent()) {
-      return new ArmPositionCommand(armSubsystem, armState.get())
+      // FIXME: we need a working armposition command here
+      return new ArmPositionCommand(armSubsystem)
           .deadlineWith(new SetOuttakeModeCommand(outtakeSubsystem, outtakeState.get()));
     } else if (armState.isPresent()) {
-      return new ArmPositionCommand(armSubsystem, armState.get());
+
+      // FIXME: we need a working armposition command here
+      return new ArmPositionCommand(armSubsystem);
     } else if (outtakeState.isPresent()) {
       return new SetOuttakeModeCommand(outtakeSubsystem, outtakeState.get());
     } else {