Implemented intake into superstructure manager and add TODOs

src/main/java/frc/robot/Robot.java

@@ -34,17 +34,19 @@ public class Robot extends LoggedRobot {
 
   private final DriveController driveController = new DriveController(0);
   private final CommandXboxController operatorController = new CommandXboxController(1);
-  private final RumbleControllerSubsystem rumbleController = new RumbleControllerSubsystem(new XboxController(1));
+  private final RumbleControllerSubsystem rumbleController =
+      new RumbleControllerSubsystem(new XboxController(1));
 
   private final FmsSubsystem fmsSubsystem = new FmsSubsystem();
+  // TODO: Add all the subsystems & managers here
 
   private final Autos autos = new Autos();
 
   private Command autoCommand;
 
   public Robot() {
 
-    //XboxController.x().onTrue(wrist.setPositionCommand(10));
+    // TODO: Start adding button bindings
 
     // Log to a USB stick
     Logger.getInstance().addDataReceiver(new WPILOGWriter("/media/sda1/"));

src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java

@@ -32,14 +32,16 @@ private void setGoal(SuperstructureState goalState) {
   }
 
   public boolean atGoal(SuperstructureState state) {
-    return shoulder.atAngle(state.position.shoulderAngle.getDegrees())
-        && wrist.atAngle(state.position.wristAngle.getDegrees());
+    return motionManager.atPosition(state.position) && intake.atGoal(state.intakeState);
   }
 
-  // in enabledperiodic, go to the goal position
   @Override
   public void enabledPeriodic() {
     motionManager.set(goalState.position);
+
+    if (motionManager.atPosition(goalState.position) || goalState.intakeNow) {
+      intake.setGoalState(goalState.intakeState);
+    }
   }
 
   public Command setStateCommand(SuperstructureState newGoalState) {

src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java

@@ -24,7 +24,7 @@ public SuperstructureMotionManager(WristSubsystem shoulder, WristSubsystem wrist
     this.wrist = wrist;
   }
 
-  private boolean atPosition(SuperstructurePosition position) {
+  public boolean atPosition(SuperstructurePosition position) {
     return shoulder.atAngle(position.shoulderAngle)
         && wrist.atAngle(position.wristAngle);
   }

src/main/java/frc/robot/managers/superstructure/SuperstructureState.java

@@ -8,13 +8,13 @@
 
 public class SuperstructureState {
   public final SuperstructurePosition position;
-  public final IntakeState intakeMode;
+  public final IntakeState intakeState;
   public final boolean intakeNow;
 
   public SuperstructureState(
       SuperstructurePosition position, IntakeState intakeMode, boolean intakeNow) {
     this.position = position;
-    this.intakeMode = intakeMode;
+    this.intakeState = intakeMode;
     this.intakeNow = intakeNow;
   }
 

src/main/java/frc/robot/wrist/WristSubsystem.java

@@ -57,7 +57,7 @@ public Command setPositionCommand(Rotation2d angle) {
         .until(() -> atAngle(angle));
   }
 
-  private boolean atAngle(Rotation2d angle) {
+  public boolean atAngle(Rotation2d angle) {
     double actualAngle = getWristAngle().getDegrees();
     return Math.abs(actualAngle - angle.getDegrees()) < 1;
   }