Start fixing scoring

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

@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+import com.ctre.phoenix.led.CANdle;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -25,13 +26,15 @@
 import frc.robot.intake.HeldGamePiece;
 import frc.robot.intake.IntakeState;
 import frc.robot.intake.IntakeSubsystem;
+import frc.robot.lights.LightsSubsystem;
 import frc.robot.localization.LocalizationSubsystem;
 import frc.robot.managers.autobalance.Autobalance;
 import frc.robot.managers.autorotate.AutoRotate;
 import frc.robot.managers.superstructure.NodeHeight;
 import frc.robot.managers.superstructure.States;
 import frc.robot.managers.superstructure.SuperstructureManager;
 import frc.robot.managers.superstructure.SuperstructureMotionManager;
+import frc.robot.managers.superstructure.SuperstructureState;
 import frc.robot.shoulder.ShoulderSubsystem;
 import frc.robot.swerve.SwerveModule;
 import frc.robot.swerve.SwerveSubsystem;
@@ -105,6 +108,8 @@ public class Robot extends LoggedRobot {
 
   private final LocalizationSubsystem localization = new LocalizationSubsystem(swerve, imu);
 
+  private final LightsSubsystem lights = new LightsSubsystem(new CANdle(Config.CANDLE_ID), intake, superstructure);
+
   private final Autos autos = new Autos(localization, swerve, intake, wrist);
 
   private Command autoCommand;
@@ -191,9 +196,10 @@ private void configureButtonBindings() {
         .onFalse(swerve.disableXSwerveCommand());
 
     // Operator controls
-    operatorController.y().onTrue(superstructure.getScoreAlignCommand(NodeHeight.HIGH));
-    operatorController.b().onTrue(superstructure.getScoreAlignCommand(NodeHeight.MID));
-    operatorController.a().onTrue(superstructure.setStateCommand(States.STOWED));
+    operatorController.y().onTrue(superstructure.getScoreAlignCommand(() -> NodeHeight.HIGH));
+    operatorController.b().onTrue(superstructure.getScoreAlignCommand(() -> NodeHeight.MID));
+    // A or X for stow
+    operatorController.a().or(operatorController.x()).onTrue(superstructure.setStateCommand(States.STOWED));
 
     // Manual intake override
     operatorController

src/main/java/frc/robot/controller/DriveController.java

@@ -46,7 +46,7 @@ public double getForwardPercentage() {
 
   /** The rotation about the robot's z-axis as a percentage (<code>-1 <= x <= 1</code>) */
   public double getThetaPercentage() {
-    return joystickScale(-1 * getRightX());
+    return joystickScale(getRightX());
   }
 
   public NodeKind getAutoScoreNodeKind() {

src/main/java/frc/robot/intake/IntakeSubsystem.java

@@ -53,12 +53,16 @@ public void robotPeriodic() {
 
   @Override
   public void enabledPeriodic() {
-    if (goalState == IntakeState.OUTTAKE_CONE) {
+    if (goalState == IntakeState.MANUAL_INTAKE) {
+      motor.set(-0.6);
+    } else if (goalState == IntakeState.MANUAL_OUTTAKE) {
+      motor.set(0.6);
+    } else if (goalState == IntakeState.OUTTAKE_CONE) {
       motor.set(0.4);
     } else if (goalState == IntakeState.OUTTAKE_CUBE) {
       motor.set(0.3);
     } else if (gamePiece == HeldGamePiece.CUBE) {
-      motor.set(-0.6);
+      motor.set(-0.75);
     } else if (gamePiece == HeldGamePiece.CONE) {
       motor.set(-0.6);
     } else if (goalState == IntakeState.INTAKE_CUBE) {

src/main/java/frc/robot/lights/LightsSubsystem.java

@@ -17,6 +17,7 @@
 import frc.robot.managers.superstructure.SuperstructureManager;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
+import org.littletonrobotics.junction.Logger;
 
 public class LightsSubsystem extends LifecycleSubsystem {
 
@@ -29,7 +30,6 @@ public class LightsSubsystem extends LifecycleSubsystem {
 
   private final Timer blinkTimer = new Timer();
   private Color color = Color.kWhite;
-  // TODO: Copy paste blink pattern enum from other repo
   private BlinkPattern blinkPattern = BlinkPattern.SOLID;
 
   public LightsSubsystem(
@@ -112,4 +112,10 @@ public void enabledPeriodic() {
       }
     }
   }
+
+  @Override
+  public void robotPeriodic() {
+    Logger.getInstance().recordOutput("Lights/BlinkPattern", blinkPattern.toString());
+    Logger.getInstance().recordOutput("Lights/Color", color.toString());
+  }
 }

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

@@ -11,7 +11,7 @@ public class Positions {
       new SuperstructurePosition(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0));
 
   public static final SuperstructurePosition INTAKING_CUBE_FLOOR =
-      new SuperstructurePosition(Rotation2d.fromDegrees(-25), Rotation2d.fromDegrees(-120));
+      new SuperstructurePosition(Rotation2d.fromDegrees(-31), Rotation2d.fromDegrees(-140));
   public static final SuperstructurePosition INTAKING_CUBE_SHELF =
       new SuperstructurePosition(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(180));
 
@@ -20,12 +20,12 @@ public class Positions {
   public static final SuperstructurePosition CUBE_NODE_LOW_BACK =
       new SuperstructurePosition(Rotation2d.fromDegrees(4.7), Rotation2d.fromDegrees(93.9));
   public static final SuperstructurePosition CUBE_NODE_MID =
-      new SuperstructurePosition(Rotation2d.fromDegrees(75), Rotation2d.fromDegrees(190));
+      new SuperstructurePosition(Rotation2d.fromDegrees(76), Rotation2d.fromDegrees(190));
   public static final SuperstructurePosition CUBE_NODE_HIGH =
       new SuperstructurePosition(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(190));
 
   public static final SuperstructurePosition INTAKING_CONE_FLOOR =
-      new SuperstructurePosition(Rotation2d.fromDegrees(-33), Rotation2d.fromDegrees(-152));
+      new SuperstructurePosition(Rotation2d.fromDegrees(-32), Rotation2d.fromDegrees(-152));
   public static final SuperstructurePosition INTAKING_CONE_SHELF =
       new SuperstructurePosition(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(180));
   public static final SuperstructurePosition INTAKING_CONE_SINGLE_SUBSTATION =
@@ -36,7 +36,7 @@ public class Positions {
   public static final SuperstructurePosition CONE_NODE_LOW_BACK =
       new SuperstructurePosition(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(89));
   public static final SuperstructurePosition CONE_NODE_MID =
-      new SuperstructurePosition(Rotation2d.fromDegrees(75), Rotation2d.fromDegrees(190));
+      new SuperstructurePosition(Rotation2d.fromDegrees(120), Rotation2d.fromDegrees(-70));
   public static final SuperstructurePosition CONE_NODE_HIGH =
       new SuperstructurePosition(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(190));
 

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

@@ -76,6 +76,17 @@ public void robotPeriodic() {
     Logger.getInstance()
         .recordOutput("Superstructure/GoalState/GoalIntakeNow", goalState.intakeNow);
     Logger.getInstance().recordOutput("Superstructure/Mode", mode.toString());
+    Logger.getInstance().recordOutput("Superstructure/ScoringProgress", scoringProgress.toString());
+    if (scoringHeight == null) {
+      Logger.getInstance().recordOutput("Superstructure/ScoringHeight", "(null)");
+    } else {
+      Logger.getInstance().recordOutput("Superstructure/ScoringHeight", scoringHeight.toString());
+    }
+    if (manualIntakeState == null) {
+      Logger.getInstance().recordOutput("Superstructure/ManualIntakeState", "(null)");
+    } else {
+      Logger.getInstance().recordOutput("Superstructure/ManualIntakeState", manualIntakeState.toString());
+    }
   }
 
   public void setIntakeOverride(IntakeState intakeState) {
@@ -193,36 +204,41 @@ private SuperstructureScoringState getScoringState(NodeHeight height) {
     }
   }
 
-  public Command getScoreAlignCommand(NodeHeight height) {
+  public Command getScoreAlignCommand(Supplier<NodeHeight> height) {
     return Commands.runOnce(
             () -> {
-              scoringHeight = height;
+              scoringHeight = height.get();
               scoringProgress = ScoringProgress.ALIGNING;
+              System.out.println("scoring height in score align command: " + scoringHeight.toString());
+              System.out.println("height.get() in score align command: " + height.get().toString());
             })
-        .andThen(setStateCommand(getScoringState(height).aligning))
+        .andThen(setStateCommand(() -> getScoringState(height.get()).aligning))
         .withName("ScoreAlignCommand");
   }
 
   public Command getScoreFinishCommand() {
-    return getScoreFinishCommand(scoringHeight == null ? NodeHeight.LOW : scoringHeight)
+    return getScoreFinishCommand(() -> scoringHeight == null ? NodeHeight.LOW : scoringHeight)
         .withName("ScoreFinishCommand");
   }
 
-  public Command getScoreFinishCommand(NodeHeight height) {
+  public Command getScoreFinishCommand(Supplier<NodeHeight> height) {
     return Commands.runOnce(
             () -> {
-              scoringHeight = height;
+              scoringHeight = height.get();
               scoringProgress = ScoringProgress.PLACING;
+              System.out.println("scoring height in score finish command: " + scoringHeight.toString());
+              System.out.println("height.get() in score finish command: " + height.get().toString());
             })
-        .andThen(setStateCommand(getScoringState(height).aligning))
-        .andThen(setStateCommand(getScoringState(height).scoring))
+        .andThen(setStateCommand(getScoringState(height.get()).aligning))
+        .andThen(setStateCommand(getScoringState(height.get()).scoring))
         .andThen(
             Commands.runOnce(
                 () -> {
-                  scoringHeight = null;
+                  System.out.println("scoring height in score finish command: " + scoringHeight.toString());
+                  System.out.println("height.get() in score finish command: " + height.get().toString());                  scoringHeight = null;
                   scoringProgress = ScoringProgress.DONE_SCORING;
                 }))
-        .andThen(stowFast().unless(() -> height != NodeHeight.LOW))
+        .andThen(stowFast().unless(() -> height.get() != NodeHeight.LOW))
         .withName("ScoreFinishCommand");
   }
 }