Fix Arm

Add working backpack

src/main/java/org/littletonrobotics/frc2024/Constants.java

@@ -21,7 +21,7 @@
  */
 public final class Constants {
   public static final double loopPeriodSecs = 0.02;
-  private static RobotType robotType = RobotType.SIMBOT;
+  private static RobotType robotType = RobotType.DEVBOT;
   public static final boolean tuningMode = true;
 
   public static RobotType getRobot() {

src/main/java/org/littletonrobotics/frc2024/RobotContainer.java

@@ -9,7 +9,6 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
@@ -28,17 +27,21 @@
 import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels;
 import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIO;
 import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSim;
+import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSparkFlex;
 import org.littletonrobotics.frc2024.subsystems.rollers.Rollers;
 import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIO;
 import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIOReal;
+import org.littletonrobotics.frc2024.subsystems.rollers.backpack.Backpack;
+import org.littletonrobotics.frc2024.subsystems.rollers.backpack.BackpackIOSim;
+import org.littletonrobotics.frc2024.subsystems.rollers.backpack.BackpackIOSparkFlex;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIO;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOKrakenFOC;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOSim;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIO;
+import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIODevbot;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSim;
-import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSparkFlex;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIO;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOKrakenFOC;
@@ -82,6 +85,7 @@ public RobotContainer() {
     Feeder feeder = null;
     Indexer indexer = null;
     Intake intake = null;
+    Backpack backpack = null;
     Arm arm = null;
 
     // Create subsystems
@@ -106,12 +110,13 @@ public RobotContainer() {
                   new AprilTagVisionIONorthstar(
                       AprilTagVisionConstants.instanceNames[1],
                       AprilTagVisionConstants.cameraIds[1]));
-          //          flywheels = new Flywheels(new FlywheelsIOSparkFlex());
+          flywheels = new Flywheels(new FlywheelsIOSparkFlex());
 
           feeder = new Feeder(new FeederIOKrakenFOC());
-          indexer = new Indexer(new IndexerIOSparkFlex());
+          indexer = new Indexer(new IndexerIODevbot());
           intake = new Intake(new IntakeIOKrakenFOC());
-          rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIOReal());
+          backpack = new Backpack(new BackpackIOSparkFlex());
+          rollers = new Rollers(feeder, indexer, intake, backpack, new RollersSensorsIOReal());
 
           arm = new Arm(new ArmIOKrakenFOC());
         }
@@ -128,7 +133,8 @@ public RobotContainer() {
           feeder = new Feeder(new FeederIOSim());
           indexer = new Indexer(new IndexerIOSim());
           intake = new Intake(new IntakeIOSim());
-          rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {});
+          backpack = new Backpack(new BackpackIOSim());
+          rollers = new Rollers(feeder, indexer, intake, backpack, new RollersSensorsIO() {});
 
           arm = new Arm(new ArmIOSim());
         }
@@ -166,7 +172,7 @@ public RobotContainer() {
       intake = new Intake(new IntakeIO() {});
     }
     if (rollers == null) {
-      rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {});
+      rollers = new Rollers(feeder, indexer, intake, backpack, new RollersSensorsIO() {});
     }
     if (arm == null) {
       arm = new Arm(new ArmIO() {});
@@ -245,7 +251,6 @@ private void configureButtonBindings() {
                 () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
     controller
         .rightTrigger()
-        .and(readyToShoot)
         .onTrue(
             Commands.parallel(
                     Commands.waitSeconds(0.5),
@@ -269,21 +274,11 @@ private void configureButtonBindings() {
                 .intake()
                 .alongWith(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor()))
                 .withName("Eject To Floor"));
+    // Test rollers with amp score
+    controller.b().whileTrue(rollers.ampScore());
 
     controller
         .y()
-        .onTrue(
-            Commands.runOnce(
-                    () ->
-                        robotState.resetPose(
-                            AllianceFlipUtil.apply(
-                                new Pose2d(
-                                    Units.inchesToMeters(36.0),
-                                    FieldConstants.Speaker.centerSpeakerOpening.getY(),
-                                    new Rotation2d()))))
-                .ignoringDisable(true));
-    controller
-        .b()
         .onTrue(
             Commands.runOnce(
                     () ->

src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java

@@ -79,7 +79,7 @@ public static Command waitUntilXCrossed(double xPosition, boolean towardsCenterl
   public static Command intake(Superstructure superstructure, Rollers rollers) {
     return parallel(
             superstructure.intake(), rollers.floorIntake().beforeStarting(superstructure::atGoal))
-        .until(rollers::hasGamepiece);
+        .until(rollers::gamepieceStaged);
   }
 
   /** Shoots note, ending after rollers have spun */

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java

@@ -255,8 +255,8 @@ public class DriveTrajectories {
         List.of(
             PathSegment.newBuilder()
                 .addPoseWaypoint(startingSourceFace)
-                .addPoseWaypoint(intakingCenterlinePoses[0])
-                .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6)))
+                .addPoseWaypoint(intakingCenterlinePoses[0], 100)
+                .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.6)), 100)
                 .build()));
 
     paths.put(

src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java

@@ -33,9 +33,9 @@ public class Flywheels extends SubsystemBase {
   private static final LoggedTunableNumber shootingRightRpm =
       new LoggedTunableNumber("Flywheels/ShootingRightRpm", 4000.0);
   private static final LoggedTunableNumber idleLeftRpm =
-      new LoggedTunableNumber("Flywheels/IdleLeftRpm", 1500.0);
+      new LoggedTunableNumber("Flywheels/IdleLeftRpm", 200);
   private static final LoggedTunableNumber idleRightRpm =
-      new LoggedTunableNumber("Flywheels/IdleRightRpm", 1000.0);
+      new LoggedTunableNumber("Flywheels/IdleRightRpm", 200);
   private static final LoggedTunableNumber intakingRpm =
       new LoggedTunableNumber("Flywheels/IntakingRpm", -2000.0);
   private static final LoggedTunableNumber ejectingRpm =

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java

@@ -11,6 +11,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import lombok.Getter;
+import lombok.Setter;
+import org.littletonrobotics.frc2024.subsystems.rollers.backpack.Backpack;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake;
@@ -21,6 +23,7 @@ public class Rollers extends SubsystemBase {
   private final Feeder feeder;
   private final Indexer indexer;
   private final Intake intake;
+  private final Backpack backpack;
 
   private final RollersSensorsIO sensorsIO;
   private final RollersSensorsIOInputsAutoLogged sensorInputs =
@@ -32,15 +35,28 @@ public enum Goal {
     STATION_INTAKE,
     EJECT_TO_FLOOR,
     QUICK_INTAKE_TO_FEED,
-    FEED_TO_SHOOTER
+    FEED_TO_SHOOTER,
+    AMP_SCORE
   }
 
-  @Getter private Goal goal = Goal.IDLE;
+  public enum GamepieceState {
+    NONE,
+    SHOOTER_STAGED
+  }
 
-  public Rollers(Feeder feeder, Indexer indexer, Intake intake, RollersSensorsIO sensorsIO) {
+  @Getter private Goal goal = Goal.IDLE;
+  @Getter @Setter private GamepieceState gamepieceState = GamepieceState.NONE;
+
+  public Rollers(
+      Feeder feeder,
+      Indexer indexer,
+      Intake intake,
+      Backpack backpack,
+      RollersSensorsIO sensorsIO) {
     this.feeder = feeder;
     this.indexer = indexer;
     this.intake = intake;
+    this.backpack = backpack;
     this.sensorsIO = sensorsIO;
 
     setDefaultCommand(runOnce(this::goIdle).withName("Rollers Idling"));
@@ -60,27 +76,25 @@ public void periodic() {
         feeder.setGoal(Feeder.Goal.IDLING);
         indexer.setGoal(Indexer.Goal.IDLING);
         intake.setGoal(Intake.Goal.IDLING);
+        backpack.setGoal(Backpack.Goal.IDLING);
       }
       case FLOOR_INTAKE -> {
         feeder.setGoal(Feeder.Goal.FLOOR_INTAKING);
         indexer.setGoal(Indexer.Goal.FLOOR_INTAKING);
         intake.setGoal(Intake.Goal.FLOOR_INTAKING);
+        backpack.setGoal(Backpack.Goal.IDLING);
         if (sensorInputs.shooterStaged) {
           indexer.setGoal(Indexer.Goal.IDLING);
+          gamepieceState = GamepieceState.SHOOTER_STAGED;
         }
       }
-      case STATION_INTAKE -> {
-        feeder.setGoal(Feeder.Goal.IDLING);
-        indexer.setGoal(Indexer.Goal.STATION_INTAKING);
-        intake.setGoal(Intake.Goal.IDLING);
-        if (sensorInputs.shooterStaged) { // TODO: add this banner sensor
-          indexer.setGoal(Indexer.Goal.IDLING);
-        }
-      }
+      case STATION_INTAKE -> {}
       case EJECT_TO_FLOOR -> {
         feeder.setGoal(Feeder.Goal.EJECTING);
         indexer.setGoal(Indexer.Goal.EJECTING);
         intake.setGoal(Intake.Goal.EJECTING);
+        backpack.setGoal(Backpack.Goal.IDLING);
+        gamepieceState = GamepieceState.NONE;
       }
       case QUICK_INTAKE_TO_FEED -> {
         feeder.setGoal(Feeder.Goal.SHOOTING);
@@ -91,20 +105,30 @@ public void periodic() {
         feeder.setGoal(Feeder.Goal.SHOOTING);
         indexer.setGoal(Indexer.Goal.SHOOTING);
         intake.setGoal(Intake.Goal.IDLING);
+        backpack.setGoal(Backpack.Goal.IDLING);
+        gamepieceState = GamepieceState.NONE;
+      }
+      case AMP_SCORE -> {
+        feeder.setGoal(Feeder.Goal.IDLING);
+        indexer.setGoal(Indexer.Goal.EJECTING);
+        intake.setGoal(Intake.Goal.IDLING);
+        backpack.setGoal(Backpack.Goal.AMP_SCORING);
+        gamepieceState = GamepieceState.NONE;
       }
     }
 
     feeder.periodic();
     indexer.periodic();
     intake.periodic();
+    backpack.periodic();
   }
 
   private void goIdle() {
     goal = Goal.IDLE;
   }
 
-  public boolean hasGamepiece() {
-    return sensorInputs.shooterStaged;
+  public boolean gamepieceStaged() {
+    return gamepieceState == GamepieceState.SHOOTER_STAGED;
   }
 
   public Command floorIntake() {
@@ -130,4 +154,8 @@ public Command feedShooter() {
         .alongWith(NoteVisualizer.shoot())
         .withName("Rollers Feed Shooter");
   }
+
+  public Command ampScore() {
+    return startEnd(() -> goal = Goal.AMP_SCORE, this::goIdle).withName("Rollers Amp Scoring");
+  }
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java

@@ -1,28 +1,35 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package org.littletonrobotics.frc2024.subsystems.rollers.backpack;
 
+import java.util.function.DoubleSupplier;
 import lombok.Getter;
 import lombok.RequiredArgsConstructor;
 import lombok.Setter;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystem;
 
-import java.util.function.DoubleSupplier;
-
 @Setter
 @Getter
 public class Backpack extends GenericRollerSystem<Backpack.Goal> {
-    @RequiredArgsConstructor
-    @Getter
-    public enum Goal implements VoltageGoal {
-        IDLING(() -> 0),
-        AMP_SCORING(new LoggedTunableNumber("Backpack/AmpScoringVoltage"));
+  @RequiredArgsConstructor
+  @Getter
+  public enum Goal implements VoltageGoal {
+    IDLING(() -> 0),
+    EJECTING(new LoggedTunableNumber("Backpack/EjectingVoltage", -12.0)),
+    AMP_SCORING(new LoggedTunableNumber("Backpack/AmpScoringVoltage", 12.0));
 
-        private final DoubleSupplier voltageSupplier;
-    }
+    private final DoubleSupplier voltageSupplier;
+  }
 
-    private Goal goal = Goal.IDLING;
+  private Goal goal = Goal.IDLING;
 
-    public Backpack(BackpackIO io) {
-        super("Backpack", io);
-    }
+  public Backpack(BackpackIO io) {
+    super("Backpack", io);
+  }
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIO.java

@@ -1,3 +1,10 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package org.littletonrobotics.frc2024.subsystems.rollers.backpack;
 
 import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystemIO;

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSim.java

@@ -1,14 +1,21 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package org.littletonrobotics.frc2024.subsystems.rollers.backpack;
 
 import edu.wpi.first.math.system.plant.DCMotor;
 import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystemIOSim;
 
 public class BackpackIOSim extends GenericRollerSystemIOSim implements BackpackIO {
-    private static final DCMotor motorModel = DCMotor.getNeoVortex(1);
-    private static final double reduction = (1.0 / 1.0);
-    private static final double moi = 0.001;
+  private static final DCMotor motorModel = DCMotor.getNeoVortex(1);
+  private static final double reduction = (1.0 / 1.0);
+  private static final double moi = 0.001;
 
-    public BackpackIOSim() {
-        super(motorModel, reduction, moi);
-    }
+  public BackpackIOSim() {
+    super(motorModel, reduction, moi);
+  }
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/BackpackIOSparkFlex.java

@@ -1,15 +1,22 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package org.littletonrobotics.frc2024.subsystems.rollers.backpack;
 
 import org.littletonrobotics.frc2024.util.drivers.rollers.GenericRollerSystemIOSparkFlex;
 
 public class BackpackIOSparkFlex extends GenericRollerSystemIOSparkFlex implements BackpackIO {
-    private static final int id = 0;
-    private static final int currentLimitAmps = 40;
-    private static final boolean invert = false;
-    private static final boolean brake = true;
-    private static final double reduction = (1.0 / 1.0);
-
-    public BackpackIOSparkFlex() {
-        super(id, currentLimitAmps, invert, brake, reduction);
-    }
+  private static final int id = 6;
+  private static final int currentLimitAmps = 40;
+  private static final boolean invert = true;
+  private static final boolean brake = true;
+  private static final double reduction = (1.0 / 1.0);
+
+  public BackpackIOSparkFlex() {
+    super(id, currentLimitAmps, invert, brake, reduction);
+  }
 }