feat: create the NarwhalUpperAssembly & made minor improvements to formatting

build.gradle

@@ -3,7 +3,7 @@ import com.github.spotbugs.snom.SpotBugsTask
 plugins {
     id "java"
     id 'pmd'
-    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.1"
     id "com.github.spotbugs" version "5.0.14"
 }
 

src/main/java/frc/robot/Constants.java

@@ -278,18 +278,18 @@ public static class NarwhalClimberConstants {
 
             public static final double GEAR_REDUCTION = 125.0;
             public static final double PULLY_REDUCTION = 10.0;
-            public static final double CLIMBER_ANGLE_TO_MOTOR_ANGLE = GEAR_REDUCTION * PULLY_REDUCTION;
+            public static final double CLIMBER_ANGLE_TO_MOTOR_ANGLE = GEAR_REDUCTION * PULLY_REDUCTION; // technically not a 1 to 1 conversion because of how the climber arm and winch are linked
 
-            public static final double PID_P = 5;
+            public static final double PID_P = 8.5;
             public static final double PID_I = 0;
             public static final double PID_D = 0.1;
             public static final double PID_F = 1.9;
 
             public static final double CLIMBER_PID_MIN_OUTPUT = -0.3;
             public static final double CLIMBER_PID_MAX_OUTPUT = 0.3;
 
-            public static final Rotation2d DEPLOYED_ANGLE = Rotation2d.fromDegrees(30);
-            public static final Rotation2d CLIMB_ANGLE = Rotation2d.fromDegrees(-5);
+            public static final Rotation2d DEPLOYED_ANGLE = Rotation2d.fromDegrees(40);
+            public static final Rotation2d CLIMB_ANGLE = Rotation2d.fromDegrees(-13.5);
         }
 
         public static class NarwhalElevatorConstants {
@@ -427,7 +427,7 @@ public static class VisionConstants {
         // Pipeline settings
         public static final int APRIL_TAG_PIPELINE = 0;
         public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout
-                .loadField(AprilTagFields.k2025Reefscape);
+                .loadField(AprilTagFields.k2025ReefscapeAndyMark);
 
         // Odometry Detection Strategy
         public static final PhotonPoseEstimator.PoseStrategy POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;

src/main/java/frc/robot/commands/narwhal/NarwhalManualClimberCommand.java

@@ -13,7 +13,7 @@ public class NarwhalManualClimberCommand extends Command {
     /**
      * Creates a manual squid climber command that toggles between down and climbed positions.
      * 
-     * @param squidClimber The climber subsystem to control.
+     * @param narwhalClimber The climber subsystem to control.
      * @param controller The Xbox controller used to drive the climber.
      */
     public NarwhalManualClimberCommand(NarwhalClimber narwhalClimber, XboxController controller) {

src/main/java/frc/robot/commands/narwhal/ManualNarwhalElevatorCommand.java → src/main/java/frc/robot/commands/narwhal/NarwhalManualElevatorCommand.java

@@ -6,7 +6,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.narwhal.NarwhalElevator;
 
-public class ManualNarwhalElevatorCommand extends Command {
+public class NarwhalManualElevatorCommand extends Command {
     private final NarwhalElevator narwhalElevator;
     private final XboxController xboxController;
     private final Supplier<Boolean> narwhalWristReadyToIntakeSupplier;
@@ -17,7 +17,7 @@ public class ManualNarwhalElevatorCommand extends Command {
      * @param controller The Xbox controller used to drive the elevator.
      * @param narwhalWristReadyToIntakeSupplier A supplier that indicates if the narwhal wrist is ready to intake.
      */
-    public ManualNarwhalElevatorCommand(
+    public NarwhalManualElevatorCommand(
             NarwhalElevator narwhalElevator, 
             XboxController controller, 
             Supplier<Boolean> narwhalWristReadyToIntakeSupplier) {

src/main/java/frc/robot/commands/narwhal/ManualNarwhalWristCommand.java → src/main/java/frc/robot/commands/narwhal/NarwhalManualWristCommand.java

@@ -4,7 +4,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.narwhal.NarwhalWrist;
 
-public class ManualNarwhalWristCommand extends Command {
+public class NarwhalManualWristCommand extends Command {
     private final XboxController xBoxController;
     private final NarwhalWrist narwhalWrist;
 
@@ -13,7 +13,7 @@ public class ManualNarwhalWristCommand extends Command {
      * @param squidClimber The climber subsystem to control.
      * @param controller The Xbox controller used to control the subsystem.
      */
-    public ManualNarwhalWristCommand(XboxController xboxController, NarwhalWrist narwhalWrist){
+    public NarwhalManualWristCommand(NarwhalWrist narwhalWrist, XboxController xboxController){
         this.xBoxController = xboxController;
         this.narwhalWrist = narwhalWrist;
         addRequirements(narwhalWrist);

src/main/java/frc/robot/subsystems/narwhal/NarwhalUpperAssembly.java

@@ -5,12 +5,28 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc.robot.commands.narwhal.NarwhalManualClimberCommand;
+import frc.robot.commands.narwhal.NarwhalManualElevatorCommand;
+import frc.robot.commands.narwhal.NarwhalManualIntakeOuttakeCommand;
+import frc.robot.commands.narwhal.NarwhalManualWristCommand;
 import frc.robot.subsystems.upper_assembly.UpperAssemblyBase;
 import frc.robot.util.upper_assembly.ScoringHeight;
 
 public class NarwhalUpperAssembly extends UpperAssemblyBase {
-
+    private final NarwhalIntakeOuttake narwhalIntakeOuttake;
+    private final NarwhalWrist narwhalWrist;
+    private final NarwhalElevator narwhalElevator;
+    private final NarwhalClimber narwhalClimber;
+
+    public NarwhalUpperAssembly(){
+        narwhalIntakeOuttake = new NarwhalIntakeOuttake();
+        narwhalWrist = new NarwhalWrist();
+        narwhalElevator = new NarwhalElevator();
+        narwhalClimber = new NarwhalClimber();
+    }
+
     public Command getCoralIntakeCommand(Supplier<Pose2d> robotPoseSupplier) {
         return new RunCommand(
             () -> {/*PLACEHOLDER, DO NOT USE RUN COMMANDS!*/},
@@ -40,10 +56,19 @@ public Command getClimbCommand(Supplier<Pose2d> robotPoseSupplier) {
     }
 
     public Command getManualCommand(XboxController controller) {
-        return new RunCommand(
-            () -> {/*PLACEHOLDER, DO NOT USE RUN COMMANDS!*/},
-            this
+        NarwhalManualIntakeOuttakeCommand narwhalManualIntakeOuttakeCommand = new NarwhalManualIntakeOuttakeCommand(narwhalIntakeOuttake, controller);
+        NarwhalManualWristCommand narwhalManualWristCommand = new NarwhalManualWristCommand(narwhalWrist, controller);
+        NarwhalManualElevatorCommand narwhalManualElevatorCommand = new NarwhalManualElevatorCommand(narwhalElevator, controller, narwhalWrist::readyToIntake);
+        NarwhalManualClimberCommand narwhalManualClimberCommand = new NarwhalManualClimberCommand(narwhalClimber, controller);
+
+        ParallelCommandGroup manualParallelCommandGroup = new ParallelCommandGroup(
+            narwhalManualIntakeOuttakeCommand,
+            narwhalManualWristCommand,
+            narwhalManualElevatorCommand,
+            narwhalManualClimberCommand
         );
+        manualParallelCommandGroup.addRequirements(this);
+        return manualParallelCommandGroup;
     }
 
     public void disable() {}

src/main/java/frc/robot/subsystems/narwhal/NarwhalWrist.java

@@ -69,6 +69,10 @@ public NarwhalWrist() {
         currentState = NarwhalWristState.STOPPED;
     }
 
+    /**
+     * Get the wrist position and detect if the position is within the WRIST_ANGLE_TOLERANCE to the INTAKE_ANGLE
+     * @return true if the wrist is within the angle tolerance to the intake angle, otherwise false.
+     */
     public boolean readyToIntake(){
         double current_position = wrist.getAbsoluteEncoder().getPosition();
         return Math.abs(current_position - NarwhalWristConstants.INTAKE_ANGLE.getRadians()) < NarwhalWristConstants.WRIST_ANGLE_TOLERANCE;
@@ -89,9 +93,6 @@ public void setCurrentMotorAngle(Rotation2d targetAngle){
      * Set the wrist motor to the intake angle (defined in constants) & update status.
      */
     public void goToIntakeAngle() {
-
-        // Inline construction of command goes here.
-        // Subsystem::RunOnce implicitly requires `this` subsystem.
         setCurrentMotorAngle(NarwhalWristConstants.INTAKE_ANGLE);
         currentState = NarwhalWristState.INTAKING; // must be after the set function because the set function will default to CUSTOM state
     }