added major changes to hood, hub tracking and added a right corner passing mode.

Author: AG-UltraViolet

src/main/java/com/swrobotics/robot/control/AimCalc.java

@@ -28,8 +28,11 @@ public class AimCalc {
     private static final double kHubObstacleRadiusMeters = Units.inchesToMeters(41.0);
 
     // TUNE THESE: The coordinates of the corner in front of the HP station
-    private static final Translation2d kBlueHumanPlayerCorner = new Translation2d(1.6, 1.45);
-    private static final Translation2d kRedHumanPlayerCorner = new Translation2d(15, 6.6);
+    private static final Translation2d kBlueHumanPlayerRightCorner = new Translation2d(1.6, 1.45);
+    private static final Translation2d kBlueHumanPlayerLeftCorner = new Translation2d(1.6, 6.6);
+    
+    private static final Translation2d kRedHumanPlayerLeftCorner = new Translation2d(15, 1.45);
+    private static final Translation2d kRedHumanPlayerRightCorner = new Translation2d(15, 6.6);
 
     // TUNE THESE: Turret physical soft limits to prevent infinite spinning/wire damage
     private static final double kTurretMinAngleDeg = -270.0; 
@@ -184,7 +187,6 @@ private Rotation2d wrapTurretAngle(Rotation2d rawTargetAngle, Rotation2d current
 
         double currentDeg = currentAngle.getDegrees();
 
-        // The turret might have a range > 360. Check which equivalent angle is closest.
         double[] possibleAngles = {
             normalizedTarget - 360.0,
             normalizedTarget,
@@ -205,16 +207,13 @@ private Rotation2d wrapTurretAngle(Rotation2d rawTargetAngle, Rotation2d current
             }
         }
 
-        // Fallback constraint just in case all angles are somehow out of bounds
         if (minError == Double.MAX_VALUE) {
             bestAngle = MathUtil.clamp(normalizedTarget, kTurretMinAngleDeg, kTurretMaxAngleDeg);
         }
 
         return Rotation2d.fromDegrees(bestAngle);
     }
-    
-    // Overloaded to maintain backwards compatibility if currentTurretAngle isn't wired yet.
-    // NOTE: Call the other update method to get wrap logic working correctly.
+
     public void update(Pose2d robotPose, ChassisSpeeds fieldSpeeds) {
         update(robotPose, fieldSpeeds, new Rotation2d());
     }
@@ -224,7 +223,7 @@ public void update(Pose2d robotPose, ChassisSpeeds fieldSpeeds, Rotation2d curre
         Translation2d hub = FieldPositions.getAllianceHubPose(alliance).getTranslation();
 
         Translation2d target = passingMode ? 
-            (alliance == DriverStation.Alliance.Blue ? kBlueHumanPlayerCorner : kRedHumanPlayerCorner) 
+            (alliance == DriverStation.Alliance.Blue ? kBlueHumanPlayerRightCorner : kRedHumanPlayerRightCorner) 
             : hub;
 
         Translation2d shooterPos = robotPose.getTranslation().plus(

src/main/java/com/swrobotics/robot/control/ControlBoard.java

@@ -89,14 +89,15 @@ private void configureControls() {
         driver.leftTrigger().whileTrue(robot.intake.commandSetState(IntakeSubsystem.State.INTAKE));
         driver.leftBumper().whileTrue(robot.indexer.commandSetState(IndexerSubsystem.State.RINDEX));
 
-        driver.rightTrigger().whileTrue(DriveCommands.shootOnTheMove(robot.drive,
-                () -> -driver.getLeftY(),
-                () -> -driver.getLeftX()
-        ).withTimeout(.5).andThen(robot.indexer.commandSetState(IndexerSubsystem.State.FEED)
-        .alongWith(robot.expansion.commandShoot())));
+        driver.rightTrigger().whileTrue(robot.shooter.commandSetState(ShooterSubsystem.State.AUTO)
+                .alongWith(robot.hood.setMode(HoodSubsystem.HoodState.AUTO_TRACK))
+                .withTimeout(.5)
+                .andThen(robot.indexer.commandSetState(IndexerSubsystem.State.FEED)
+                        .alongWith(robot.expansion.commandShoot())));
 
         driver.rightBumper()
-                .whileTrue(commandSetRoboState(ControlBoard.RoboState.PASS)
+                .whileTrue(robot.shooter.commandSetState(ShooterSubsystem.State.PASS)
+                .alongWith(robot.hood.setMode(HoodSubsystem.HoodState.PASSING))
                 .withTimeout(.75)
                 .andThen(robot.indexer.commandSetState(IndexerSubsystem.State.FEED)
                 .alongWith(robot.expansion.commandShoot())));

src/main/java/com/swrobotics/robot/subsystems/shooter/ShooterSubsystem.java

@@ -42,7 +42,7 @@ public ShooterSubsystem() {
 
         config.apply(motorL, motorR);
         motorR.setControl(new Follower(motorL.getDeviceID(), MotorAlignmentValue.Opposed));
-        setDefaultCommand(commandSetState(State.PASS));
+        setDefaultCommand(commandSetState(State.IDLE));
     }
 
     @Override

src/main/java/com/swrobotics/robot/subsystems/shooter/hood/HoodSubsystem.java

@@ -24,7 +24,7 @@ public class HoodSubsystem extends SubsystemBase {
     private final VoltageOut voltageControl = new VoltageOut(0);
 
     public enum HoodState { HOMING, IDLE, AUTO_TRACK, PASSING, MANUAL }
-    private HoodState state = HoodState.PASSING;
+    private HoodState state = HoodState.HOMING;
 
     private double targetRotations = kMinAngleRot;
     private final Timer homingTimer = new Timer();