Merge branch 'main' of github.com:FairportRobotics/Castor2026

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -7,17 +12,20 @@
           "incKey": 68
         },
         {
-          "decKey": 87,
+          "decKey": 83,
-          "incKey": 83
+          "incKey": 87
         },
         {
-          "decKey": 69,
+          "decayRate": 0.009999999776482582,
-          "decayRate": 0.0,
-          "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 81,
+          "incKey": 69
         }
       ],
-      "axisCount": 3,
+      "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
         90,
@@ -91,7 +99,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "030000004c050000cc09000011810000"
+      "guid": "Keyboard0"
     }
   ]
 }

src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto

@@ -9,6 +9,25 @@
           "data": {
             "pathName": "S. Hub. shoot 8. E-Hub"
           }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 10.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "ChassisAlignShoot"
+                }
+              }
+            ]
+          }
         }
       ]
     }

src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path

@@ -32,12 +32,12 @@
     },
     {
       "anchor": {
-        "x": 3.473,
+        "x": 2.9483700538099855,
-        "y": 4.013
+        "y": 4.00221686746988
       },
       "prevControl": {
-        "x": 3.1233132530120478,
+        "x": 2.5986833068220334,
-        "y": 4.034855421686746
+        "y": 4.024072289156626
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -1,30 +1,39 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
-
 package frc.robot;
 
-import edu.wpi.first.units.Units;
-import edu.wpi.first.units.measure.Angle;
-
 import com.ctre.phoenix6.signals.InvertedValue;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.units.measure.Angle;
+
 /**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * The Constants class provides a convenient place for teams to hold robot-wide
- * constants. This class should not be used for any other purpose. All constants should be declared
+ * numerical or boolean constants. This class should not be used for any other
- * globally (i.e. public static). Do not put anything functional in this class.
+ * purpose. All constants should be declared globally (i.e. public static). Do
+ * not put anything functional in this class.
  *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * <p>
- * constants are needed, to reduce verbosity.
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the constants are needed, to reduce verbosity.
  */
 public final class Constants {
+
     public static class OperatorConstants {
+
         public static final int kDriverControllerPort = 0;
     }
 
     public static class CameraConstanst {
 
         public static class IDFilters {
+
             public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11};
             public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24};
         }
@@ -36,10 +45,13 @@ public final class Constants {
 
         public static final String FRONT_CAMERA_NAME = "limelight-front";
         public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
-  
+        public static final double[] BLUE_ZONE_COOR = new double[]{1};
+        public static final double[] RED_ZONE_COOR = new double[]{1};
+        public static final double[] NEUTRAL_ZONE_COOR = new double[]{1};
     }
 
     public static class AdvantageKitConstants {
+
         public enum RobotType {
             SIM,
             REAL,
@@ -48,12 +60,15 @@ public final class Constants {
         public static final RobotType CURRENT_MODE = RobotType.REAL;
     }
     // Swerve offsets as of 3/2/26
+
     public static class SwerveDriveOffsets {
+
         public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
         public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
         public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
         public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
     }
+
     /*
    * Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
    * Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
@@ -63,6 +78,7 @@ public final class Constants {
    * Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
      */
     public static class SwerveDriveIDs {
+
         public static final int FRONT_LEFT_STEER_ID = 11;
         public static final int FRONT_LEFT_DRIVE_ID = 12;
         public static final int FRONT_LEFT_ENCODER_ID = 13;
@@ -76,12 +92,28 @@ public final class Constants {
         public static final int BACK_RIGHT_DRIVE_ID = 42;
         public static final int BACK_RIGHT_ENCODER_ID = 43;
     }
+
+    public static class RobotChassisLimits {
+        // Max velocity in meters per second
+
+        public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
+        // In meters per second squared
+        public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
+
+        // In radians per second
+        public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI;
+        // In radians per second squared
+        public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI;
+    }
+
     //Bird
     public static class ExtraIDEntities {
+
         public static final int PIGEON_ID = 1;
     }
 
     public static class IntakeConstants {
+
         public static final int DEPLOY_MOTOR_ID = 17;
         public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
 
@@ -90,6 +122,7 @@ public final class Constants {
     }
 
     public static class ShooterConstants {
+
         public static final int LAUNCHER_MOTOR_ID = 3;
         public static final boolean LAUNCHER_MOTOR_INVERTED = false;
 
@@ -99,7 +132,7 @@ public final class Constants {
         public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
         public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
         public static final double HOMING_SPEED = .35;
-    public static final double TURRET_GEAR_RATIO  = 1;
+        public static final double TURRET_GEAR_RATIO = 6;
 
         public static final int HOOD_SERVO_CHANNEL = 0;
         public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
@@ -110,15 +143,25 @@ public final class Constants {
         public static final double DEFLECTOR_STORED_ANGLE = 0;
         public static final double DEFLECTOR_SET_ANGLE1 = 100;
         public static final double DEFLECTOR_SET_ANGLE2 = 200;
-    public static final double DEFLECTOR_SET_ANGLE3 = 300;
+        public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed
         public static final double DEFLECTOR_SERVO_RATIO = 1.0;
     }
 
     public static class HopperConstants {
+
         public static final int KICKER_MOTOR_ID = 9;
-    public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
+        public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
 
         public static final int SPINDEXER_MOTOR_ID = 8;
         public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
     }
+
+    public static class FieldPoses {
+
+        private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+        public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
+        public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
+
+    }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -1,37 +1,40 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
-
 package frc.robot;
 
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.AutoShootCommandChassisTurret;
-import frc.robot.commands.ManualHopperCommand;
+import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
-import frc.robot.commands.ManualIntake;
+import frc.robot.commands.RelayCommand;
-import frc.robot.commands.ManualShootCommand;
 import frc.robot.commands.SetDeflectorCommand;
+import frc.robot.commands.ZoneCheck;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
 
 /**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * Command-based is a "declarative" paradigm, very little robot logic should
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * actually be handled in the {@link Robot} periodic methods (other than the
- * subsystems, commands, and trigger mappings) should be declared here.
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and trigger mappings) should be declared here.
  */
 public class RobotContainer {
     // The robot's subsystems and commands are defined here...
     // Replace with CommandPS4Controller or CommandJoystick if needed
-  private final CommandXboxController m_driverController =
+
-      new CommandXboxController(OperatorConstants.kDriverControllerPort);
+    private final CommandXboxController m_driverController
+            = new CommandXboxController(OperatorConstants.kDriverControllerPort);
 
     // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
-
     public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
 
     public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
@@ -40,30 +43,48 @@ public class RobotContainer {
 
     public final HopperSubsystem hopperSubsystem = new HopperSubsystem();
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+    /**
+     * The container for the robot. Contains subsystems, OI devices, and
+     * commands.
+     */
     public RobotContainer() {
+
+        // turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
+        // turretSubsystem.homeTurret();
         // Configure the trigger bindings
         configureBindings();
+
+        NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+
     }
 
     /**
-   * Use this method to define your trigger->command mappings. Triggers can be created via the
+     * Use this method to define your trigger->command mappings. Triggers can be
-   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
+     * created via the
-   * predicate, or via the named factories in {@link
+     * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor
+     * with an arbitrary predicate, or via the named factories in {@link
      * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
      * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
      * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
      * joysticks}.
+     *
      */
-
     private void configureBindings() {
-    turretSubsystem.startHoming();
         m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
         m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
         m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
         m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
 
-    m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+        m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
+        //m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
+        // m_driverController:wa.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+
+        m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
+        m_driverController.rightBumper().onTrue(intakeSubsystem.intake());
+        m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
+        m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
+        m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+        m_driverController.b().toggleOnTrue(new ZoneCheck());
     }
 
     /**
@@ -73,6 +94,8 @@ public class RobotContainer {
      */
     public Command getAutonomousCommand() {
         // An example command will be run in autonomous
-    return driveSubsystem.getAutoCommand();
+        return Commands.parallel(intakeSubsystem.deploy(), Commands.run(() -> {
+            turretSubsystem.homeTurret();
+        }, turretSubsystem), driveSubsystem.getAutoCommand());
     }
 }

src/main/java/frc/robot/Utils.java

@@ -0,0 +1,25 @@
+package frc.robot;
+
+public class Utils {
+    
+    /**
+     * Calculates the needed angle of the turret for a distance from the HUB.
+     * @param distanceToTag
+     * @return number of rotations of the turret, robot relative
+     */
+    public static double calculateTurretAngleFromCameraTagDistance(double distanceToTag){
+
+        // Calculate turret Angle
+        double cameraDistanceToTarget = distanceToTag + .6;
+        double cameraToShootDistance = 0.26035;
+        // a^2 + b^2 = c^2
+        double shooterDistanceToTarget = Math
+                .sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
+        double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
+        double robotTurretAngle = (Math.PI / 2) - angleAtShooter;
+        double robotTurretRotations = (robotTurretAngle / Math.PI);
+
+        return robotTurretRotations;
+    }
+
+}

src/main/java/frc/robot/commands/AnyTarget.java

@@ -0,0 +1,144 @@
+package frc.robot.commands;
+
+import java.util.Arrays;
+import java.util.List;
+
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.Utils;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class AnyTarget extends Command {
+
+    // For Chassis Control
+    private DriveSubsystem driveSubsystem;
+
+    private HopperSubsystem hopperSubsystem;
+    private TurretSubsystem turretSubsystem;
+
+    private PIDController cameraAutoCenterController;
+    private PIDController deadreckoningAutoCenterController;
+
+    private int[] tagFilters;
+
+    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+    private Pose3d closestTag;
+
+    private final double chassisRotateSpeed = Math.PI * 0.1;
+
+    public AnyTarget(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) {
+        this.driveSubsystem = driveSubsystem;
+        this.hopperSubsystem = hopperSubsystem;
+        this.turretSubsystem = turretSubsystem;
+
+        this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
+
+        cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0);
+        cameraAutoCenterController.setTolerance(0.2); // Units degrees
+        cameraAutoCenterController.setSetpoint(0);
+
+        deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0);
+        deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations
+        deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI);
+    }
+
+    @Override
+    public void initialize() {
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
+            } else {
+                tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
+            }
+        });
+
+        turretSubsystem.setLauncher(6000);
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
+        hopperSubsystem.spindexerOn();
+        cameraAutoCenterController.calculate(-100);
+
+        // Change to HUB tracking pipeline
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER);
+        LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
+
+        Pose3d botPose = driveSubsystem.getBotPose();
+        // Find closest target to us
+        List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList();
+        closestTag = botPose.nearest(tagPoses);
+        Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closestTag);
+
+        Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
+        Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
+
+        deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians());
+    }
+
+    @Override
+    public void execute() {
+        // Need to compute shoot speed and angle
+
+        Logger.recordOutput("AutoAlign-CenteringError", cameraAutoCenterController.getError());
+
+        // No april tag in camera view. Rotate towards our theoretical closest tag
+        if (!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)) {
+            Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
+
+            Pose3d botPose = driveSubsystem.getBotPose();
+            //driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians()));
+        } else // We have an april tag, center it to the camera frame
+        {
+            Logger.recordOutput("AutoAlignState-Tracking", "Camera");
+            // [tx, ty, tz, pitch, yaw, roll]
+            double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
+            if (targetsPose.length >= 1) {
+                Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
+
+                // Calculate turret Angle
+                double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
+                turretSubsystem.setTurretMotorRotation(-turretAngle);
+
+                driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
+            }
+        }
+
+        // If robot is centered on HUB and shooter is up to speed. Run the kicker
+        if (turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()) {
+            Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
+            hopperSubsystem.feedKicker();
+        } else {
+            Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        turretSubsystem.setLauncher(0);
+
+        hopperSubsystem.stopKicker();
+        hopperSubsystem.spindexerOff();
+
+        driveSubsystem.stopDrive();
+
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
+    }
+
+}

src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java

@@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
+import frc.robot.Utils;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
@@ -40,7 +41,7 @@ public class AutoShootCommandChassisTurret extends Command{
 
     private int[] tagFilters;
 
-    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
+    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
 
     private Pose3d closestTag;
 
@@ -54,8 +55,8 @@ public class AutoShootCommandChassisTurret extends Command{
 
         this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
 
-        cameraAutoCenterController = new PIDController(0.1, 0, 0.0);
+        cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0);
-        cameraAutoCenterController.setTolerance(0.2); // Units is meters
+        cameraAutoCenterController.setTolerance(0.2); // Units degrees
         cameraAutoCenterController.setSetpoint(0);
 
 
@@ -74,12 +75,13 @@ public class AutoShootCommandChassisTurret extends Command{
             }
         });
 
-        turretSubsystem.setLauncher(2500);
+        turretSubsystem.setLauncher(6000);
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
         hopperSubsystem.spindexerOn();
         cameraAutoCenterController.calculate(-100);
 
         // Change to HUB tracking pipeline
-        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER);
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER);
         LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
 
         Pose3d botPose = driveSubsystem.getBotPose();
@@ -91,7 +93,7 @@ public class AutoShootCommandChassisTurret extends Command{
         Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
         Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
 
-        deadreckoningAutoCenterController.setSetpoint(delta.getAngle().getRadians());
+        deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians());
     }
 
     @Override
@@ -102,25 +104,33 @@ public class AutoShootCommandChassisTurret extends Command{
 
         // No april tag in camera view. Rotate towards our theoretical closest tag
         if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){ 
-            Logger.recordOutput("AutoAlignState", "Look for tag");
+            Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
 
             Pose3d botPose = driveSubsystem.getBotPose();
-            driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this
+            //driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians()));
         }
         else // We have an april tag, center it to the camera frame
         {
-            Logger.recordOutput("AutoAlignState", "Center on tag");
+            Logger.recordOutput("AutoAlignState-Tracking", "Camera");
             // [tx, ty, tz, pitch, yaw, roll]
             double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
             if(targetsPose.length >= 1){
+                Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
+
+                // Calculate turret Angle
+                double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
+                turretSubsystem.setTurretMotorRotation(-turretAngle);
+
                 driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
             }
         }
 
         // If robot is centered on HUB and shooter is up to speed. Run the kicker
         if(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){
-            Logger.recordOutput("AutoAlignState", "FIRE!");
+            Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
             hopperSubsystem.feedKicker();
+        }else{
+            Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
         }
     }
 

src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java

@@ -0,0 +1,130 @@
+package frc.robot.commands;
+
+import java.util.Arrays;
+import java.util.List;
+
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+import org.littletonrobotics.junction.Logger;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Constants;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class AutoShootCommandChassisTurretPathPlanner extends Command {
+
+    // For Chassis Control
+    private DriveSubsystem driveSubsystem;
+
+    private HopperSubsystem hopperSubsystem;
+    private TurretSubsystem turretSubsystem;
+
+    private int[] tagFilters;
+
+    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+    private Pose3d closestTag;
+    private Pose2d targetPPPose;
+
+    private Command pathFollowCommand;
+
+    public AutoShootCommandChassisTurretPathPlanner(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem,
+            TurretSubsystem turretSubsystem) {
+        this.driveSubsystem = driveSubsystem;
+        this.hopperSubsystem = hopperSubsystem;
+        this.turretSubsystem = turretSubsystem;
+
+        this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
+
+    }
+
+    @Override
+    public void initialize() {
+
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            Pose3d targetHub;
+            if (aliance == Alliance.Blue) {
+                tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
+                targetHub = Constants.FieldPoses.BLUE_HUB_POSE;
+            } else {
+                tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
+                targetHub = Constants.FieldPoses.RED_HUB_POSE;
+            }
+
+            Pose3d botPose = driveSubsystem.getBotPose();
+            // Find closest target to us
+            List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get())
+                    .toList();
+            closestTag = botPose.nearest(tagPoses);
+            Logger.recordOutput("AutoAlignPP-ClosestAprilTagPose", closestTag);
+
+            Translation2d delta = botPose.getTranslation().toTranslation2d()
+                    .minus(closestTag.getTranslation().toTranslation2d());
+            Logger.recordOutput("AutoAlignPP-ClosestAprilTagTransform", delta);
+
+            // Add 180 because we want the back of the robot to face the HUB
+            // deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
+
+            // Translate back 1 meter from HUB
+            Translation2d newTrans = targetHub.getTranslation().toTranslation2d()
+                    .plus(new Translation2d(1.5, delta.getAngle()));
+            targetPPPose = new Pose2d(newTrans, delta.getAngle());
+            Logger.recordOutput("AutoAlignPP-PPTargetPose", targetPPPose);
+
+            pathFollowCommand = AutoBuilder.pathfindToPose(targetPPPose,
+                    new PathConstraints(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY,
+                            Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL,
+                            Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY,
+                            Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL));
+
+            CommandScheduler.getInstance().schedule(pathFollowCommand);
+        });
+
+        turretSubsystem.setLauncher(5500);
+        hopperSubsystem.spindexerOn();
+
+    }
+
+    @Override
+    public void execute() {
+
+        // If robot is centered on HUB and shooter is up to speed. Run the kicker
+        if (turretSubsystem.isLauncherUpToSpeed() && pathFollowCommand.isFinished()) {
+            Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
+            hopperSubsystem.feedKicker();
+        } else {
+            Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        CommandScheduler.getInstance().cancel(pathFollowCommand);
+        turretSubsystem.setLauncher(0);
+
+        hopperSubsystem.stopKicker();
+        hopperSubsystem.spindexerOff();
+
+        driveSubsystem.stopDrive();
+
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME,
+                Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
+    }
+
+}

src/main/java/frc/robot/commands/AutoTurretCommand.java

@@ -0,0 +1,88 @@
+package frc.robot.commands;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class AutoTurretCommand extends Command {
+
+    private final Translation2d CENTER_TURRET_TO_ROBOT = new Translation2d(-0.1335024, -0.1824736);
+
+    private TurretSubsystem turretSubsystem;
+    private DriveSubsystem driveSubsystem;
+
+    private int[] shootingTagFilters;
+
+    private Pose3d closestTag;
+    private Pose3d scoringHubPose;
+
+    public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
+        this.turretSubsystem = turretSubsystem;
+        this.driveSubsystem = driveSubsystem;
+
+        addRequirements(turretSubsystem); // Not adding drive as we don't wan't the drive command to be de-scheduled
+    }
+
+    @Override
+    public void initialize() {
+        // Perform auto-home
+        turretSubsystem.homeTurret();
+
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
+                scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE;
+            } else {
+                shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
+                scoringHubPose = Constants.FieldPoses.RED_HUB_POSE;
+            }
+            Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose);
+        });
+
+    }
+
+    @Override
+    public void execute() {
+
+        Pose3d botPose = driveSubsystem.getBotPose();
+        Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
+                new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative()))));
+        Logger.recordOutput("AutoTurret-TurretPose", turretPose);
+
+        Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
+        Logger.recordOutput("AutoTurret-Translation", targetTranslation);
+
+
+        if(turretSubsystem.isTurretReady()){
+            // turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
+            // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
+        }
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+}

src/main/java/frc/robot/commands/ExampleCommand.java

@@ -4,25 +4,21 @@
 
 package frc.robot.commands;
 
-import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.ExampleSubsystem;
-
-import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 
 /** An example command that uses an example subsystem. */
-public class ManualIntake extends Command {
+public class ExampleCommand extends Command {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final IntakeSubsystem m_subsystem;
+  private final ExampleSubsystem m_subsystem;
-  private final XboxController m_XboxController;
 
   /**
    * Creates a new ExampleCommand.
    *
    * @param subsystem The subsystem used by this command.
    */
-  public ManualIntake(IntakeSubsystem subsystem, XboxController xboxController) {
+  public ExampleCommand(ExampleSubsystem subsystem) {
     m_subsystem = subsystem;
-    m_XboxController = xboxController;
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(subsystem);
   }
@@ -33,12 +29,7 @@ public class ManualIntake extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
+  public void execute() {}
-    if(m_XboxController.getYButton()==true)
-    {
-      m_subsystem.extend();
-    }
-  }
 
   // Called once the command ends or is interrupted.
   @Override

src/main/java/frc/robot/commands/ManualHopperCommand.java

@@ -1,58 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import frc.robot.subsystems.HopperSubsystem;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-
-/** An example command that uses an example subsystem. */
-public class ManualHopperCommand extends Command {
-  private final HopperSubsystem m_subsystem;
-  private final XboxController m_xboxController;
-
-  public ManualHopperCommand(HopperSubsystem subsystem, XboxController xboxController) {
-    m_subsystem = subsystem;
-    m_xboxController = xboxController;
-    addRequirements(subsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    m_subsystem.spindexerOn();
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2)
-    {
-      m_subsystem.feedKicker();
-    }
-    else
-    {
-      m_subsystem.stopKicker();
-    }
-    if(m_xboxController.getRightBumper())
-    {
-      m_subsystem.spindexerOn();
-    }
-    if(m_xboxController.getLeftBumper())
-    {
-      m_subsystem.spindexerOff();
-    }
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

src/main/java/frc/robot/commands/ManualShootCommand.java

@@ -1,48 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import frc.robot.subsystems.TurretSubsystem;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj2.command.Command;
-
-/** An example command that uses an example subsystem. */
-public class ManualShootCommand extends Command {
-  private final TurretSubsystem m_subsystem;
-  private final XboxController m_controller;
-
-  public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) {
-    m_subsystem = subsystem;
-    m_controller = controller;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    if(m_controller.getRightTriggerAxis()>.5 && m_controller.getLeftTriggerAxis()==0)
-    {
-      m_subsystem.startRotate(-.5);
-      m_controller.setRumble(RumbleType.kRightRumble, 1);
-    }
-    else if(m_controller.getLeftTriggerAxis()>.5 && m_controller.getRightTriggerAxis()==0)
-    {
-      m_subsystem.startRotate(.5);
-      m_controller.setRumble(RumbleType.kLeftRumble, 1);
-    }
-    else
-    {
-      m_subsystem.startRotate(0);
-      m_controller.setRumble(RumbleType.kBothRumble, 0);
-    }
-  }
-
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

src/main/java/frc/robot/commands/RelayCommand.java

@@ -0,0 +1,63 @@
+package frc.robot.commands;
+
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class RelayCommand extends Command{
+
+    private HopperSubsystem hopperSubsystem;
+    private TurretSubsystem turretSubsystem;
+    private IntakeSubsystem intakeSubsystem;
+
+    private Command waitCommand = Commands.waitSeconds(1.5);
+
+    public RelayCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem){
+        this.hopperSubsystem = hopperSubsystem;
+        this.turretSubsystem = turretSubsystem;
+        this.intakeSubsystem = intakeSubsystem;
+
+        addRequirements(hopperSubsystem, turretSubsystem, intakeSubsystem);
+    }
+    
+    @Override
+    public void initialize() {
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3);
+        turretSubsystem.setLauncher(6000);
+        hopperSubsystem.spindexerOn();
+        CommandScheduler.getInstance().schedule(waitCommand);
+        turretSubsystem.setTurretMotorRotation(1); // Return to 0 after MIAMI VALLEY
+    }
+
+    @Override
+    public void execute() {
+        if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
+            hopperSubsystem.feedKicker();
+        }else{
+            hopperSubsystem.stopKicker();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        waitCommand.cancel();
+        hopperSubsystem.spindexerOff();
+        hopperSubsystem.stopKicker();
+
+        turretSubsystem.setLauncher(0);
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
+        turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
+    }
+
+}

src/main/java/frc/robot/commands/ZoneCheck.java

@@ -0,0 +1,79 @@
+package frc.robot.commands;
+
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+
+public class ZoneCheck extends Command {
+
+    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+    private Zone zone;
+
+    public enum Zone {
+        Blue,
+        Red,
+        Neutral;
+    }
+
+    public ZoneCheck() {
+        getBotPose();
+    }
+
+    @Override
+    public void initialize() {
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                switch (zone) {
+                    case Blue:
+                        System.out.println("Im Home");
+                    case Neutral:
+                        System.out.println("Uhhhhhhhh not red");
+                        ;
+                    default:
+                        System.out.println("BOOOOO, BOOOO RED");
+                        ;
+                }
+            } else {
+                switch (zone) {
+                    case Blue:
+                        System.out.println("Screw Blue");
+                        ;
+                    case Neutral:
+                        System.out.println("Uhhhhhhhh not red");
+                        ;
+                    default:
+                        System.out.println("Im Home");
+                }
+            }
+        });
+    }
+
+    @Override
+    public void execute() {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    private double[] getBotPose() {
+        return LimelightHelpers.getBotPose_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
+    }
+
+    private void getZone() {
+        if (getBotPose()[0] >= Constants.CameraConstanst.RED_ZONE_COOR[0]) {
+            zone = Zone.Red;
+        } else if (getBotPose()[0] >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) {
+            zone = Zone.Neutral;
+        } else {
+            zone = Zone.Blue;
+        }
+    }
+}

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -10,6 +10,8 @@ import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
 import org.littletonrobotics.junction.Logger;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
@@ -50,8 +52,8 @@ public class DriveSubsystem extends TestableSubsystem {
         driveSystem = swerveBuilder
                 .withCanbusName("Drive")
                 .withPigeonId(Constants.ExtraIDEntities.PIGEON_ID)
-                .withMaxLinearVelocity(3.0)
+                .withMaxLinearVelocity(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY)
-                .withMaxAngularVelocity(Math.PI)
+                .withMaxAngularVelocity(Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY)
                 .withSwerveModule(
                         swerveBuilder.new SwerveModuleBuilder()
                                 .withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID)
@@ -133,9 +135,8 @@ public class DriveSubsystem extends TestableSubsystem {
                                 .build())
                 .build();
 
-
         // Set vision measurement confidence values
-        driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999));
+        driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999));
 
         this.setDefaultCommand(Commands.run(new Runnable() {
 
@@ -163,8 +164,8 @@ public class DriveSubsystem extends TestableSubsystem {
                     driveSystem::getRobotRelativeSpeeds,
                     (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
                     new PPHolonomicDriveController(
-                            new PIDConstants(5.0, 0, 0),
+                            new PIDConstants(1.5, 0, 0),
-                            new PIDConstants(5.0, 0, 0)),
+                            new PIDConstants(Math.PI / 2, 0, 0)),
                     config,
                     () -> {
                         var alliance = DriverStation.getAlliance();

src/main/java/frc/robot/subsystems/HopperSubsystem.java

@@ -6,36 +6,84 @@ package frc.robot.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
 import org.fairportrobotics.frc.posty.test.PostTest;
+import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 
+import edu.wpi.first.units.AngularVelocityUnit;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.AngularVelocity;
+
 import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 import frc.robot.Constants;
 
 public class HopperSubsystem extends TestableSubsystem {
 
     private TalonFX spindexerMotor;
+
+
     private TalonFX kickerMotor;
+    private VelocityVoltage kickerControl = new VelocityVoltage(0);
+    private StatusSignal<AngularVelocity> kickerVelocity;
+
   /** Creates a new HopperSubsystem. */
   public HopperSubsystem() {
     spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
-    spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION));
+    spindexerMotor.getConfigurator().apply(
+        new TalonFXConfiguration()
+            .withMotorOutput(
+                new MotorOutputConfigs()
+                    .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
+            .withSlot0(
+                new Slot0Configs()
+                    .withKP(0)
+                    .withKI(0)
+                    .withKD(0)
+                    .withKS(0)));
 
     kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
-    kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION));
+    kickerMotor.getConfigurator().apply(
+        new TalonFXConfiguration()
+            .withMotorOutput(
+                new MotorOutputConfigs()
+                    .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
+            )
+            .withSlot0(new Slot0Configs()
+              .withKP(0)
+              .withKI(0)
+              .withKD(0)
+              .withKS(0)
+            ));
+    kickerVelocity = kickerMotor.getVelocity();
   } 
 
-  public void feedKicker() {kickerMotor.set(0.5);}
+  public void feedKicker() {
+    kickerMotor.setControl(kickerControl.withVelocity(1000));
+    // kickerMotor.set(0.75);
+  }
 
-  public void reverseKicker() {kickerMotor.set(-0.5);} 
+  public void reverseKicker() {
+    kickerMotor.setControl(kickerControl.withVelocity(-500));
+    // kickerMotor.set(-0.5);
+  } 
 
   public void stopKicker() {kickerMotor.stopMotor();}
 
-  public void spindexerOn() { spindexerMotor.set(0.5);}
+  public void spindexerOn() { spindexerMotor.set(0.75);}
 
   public void spindexerOff() { spindexerMotor.stopMotor();}
 
+  @Override
+  public void periodic() {
+      Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
+  }
+
   @PostTest(enabled = true)
   public void hopperSystem_CANDevicesConnected(){
     assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();

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

@@ -17,6 +17,7 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 import static org.fairportrobotics.frc.robolib.motors.Utils.*;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 
 public class IntakeSubsystem extends TestableSubsystem {
   /** Creates a new ExampleSubsystem. */
@@ -35,9 +36,21 @@ public class IntakeSubsystem extends TestableSubsystem {
     deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
   }
 
-  public void extend()
+  public Command deploy()
   {
-    deployMotor.set(1);
+    return Commands.sequence(
+      Commands.deadline(
+        Commands.waitSeconds(2),
+        this.runOnce(
+          () -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy
+        )
+      ),
+      this.runOnce(() -> deployMotor.stopMotor())
+    );
+  }
+
+  public Command resetDeploy(){
+    return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
   }
 
   public void setSpeed(double speed)
@@ -45,19 +58,14 @@ public class IntakeSubsystem extends TestableSubsystem {
     intakeMotor.set(speed);
   }
 
-  public Command startSpeedCommand()
+  public Command intake()
   {
-    return this.runOnce(() -> setSpeed(-.5));
+    return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
   }
 
-  public Command killSpeedCommand()
+  public Command reverseIntake()
   {
-    return this.runOnce(() -> setSpeed(0));
+    return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();});
-  }
-
-  public Command revSpeedCommand()
-  {
-    return this.runOnce(() -> setSpeed(.5));
   }
 
   @PostTest(enabled = true)

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -11,15 +11,22 @@ import org.littletonrobotics.junction.Logger;
 import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 
 import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.ForwardLimitValue;
 import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.ReverseLimitValue;
+import com.ctre.phoenix6.sim.TalonFXSimState;
 import com.revrobotics.PersistMode;
 import com.revrobotics.ResetMode;
 import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
@@ -27,29 +34,36 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 import frc.robot.Constants;
+import edu.wpi.first.units.AngleUnit;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 
 public class TurretSubsystem extends TestableSubsystem {
   public enum TurretState {
     INIT,
     HOMING,
-    MANUAL;
+    READY;
   }
 
   private final boolean LOGGING = true;
-  private TurretState currentState;
+  private TurretState turretState;
   private DigitalInput turretLimitSwitch;
   private TalonFX turretMotor;
+  private PositionVoltage turretControlMode = new PositionVoltage(0);
+  private TalonFXSimState turretSim;
+  private StatusSignal<Angle> turretMotorPosition;
+  private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
+  private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
   private SparkMax launcherMotor;
   private SparkClosedLoopController launcherControler;
 
+  private double turretOffsetInRotations = 0;
+
   private Servo hood;
-  private Angle limitPos;
-  private Angle limitNeg;
-  private boolean turretGoingPos;
 
   public TurretSubsystem() {
 
@@ -60,9 +74,10 @@ public class TurretSubsystem extends TestableSubsystem {
       new TalonFXConfiguration()
         .withSlot0(
           new Slot0Configs()
-            .withKP(1.0)
+            .withKP(6)
             .withKI(0)
-            .withKD(0)
+            .withKD(0.3)
+            .withKS(0)
         )
         .withMotorOutput(
           new MotorOutputConfigs()
@@ -70,35 +85,53 @@ public class TurretSubsystem extends TestableSubsystem {
         )
         .withCurrentLimits(
           new CurrentLimitsConfigs()
+            .withStatorCurrentLimit(50)
+        )
+        .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(false).withReverseSoftLimitEnable(false))
+        .withFeedback(
+          new FeedbackConfigs()
+            .withFeedbackRotorOffset(0)
         )
     );
+    turretMotorPosition = turretMotor.getPosition();
+    turretMotorForwardLimit = turretMotor.getForwardLimit();
+    turretMotorReverseLimit = turretMotor.getReverseLimit();
+
+    turretSim = turretMotor.getSimState();
 
     // Launcher configuration
     launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless);
     SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
     config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
-    config.closedLoop.p(1).i(0).d(0.1);
+    config.voltageCompensation(10);
-    config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
+    config.closedLoop.p(0.0001).i(0).d(0.0); // I = 0.0000001
+    config.closedLoop.feedForward.kS(0.2).kV(0.002).kA(0.00);
+    // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
+    config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+    config.closedLoop.positionWrappingEnabled(false);
+    // config.closedLoopRampRate(0.5);
+    // config.smartCurrentLimit(5, 50);
     launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
     launcherControler = launcherMotor.getClosedLoopController();
 
     // Hood configuration
     hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL);
-    currentState = TurretState.INIT;
+    turretState = TurretState.INIT;
-    limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG;
-    limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS;
-    turretGoingPos = false;
   }
 
-  public void setLauncher(double speed) {launcherControler.setSetpoint(speed, ControlType.kVelocity);}
+  public void setLauncher(double speed) {
+    if(speed == 0)
+    {
+        launcherMotor.stopMotor();
+        return;
+    }
+    launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0);
+  }
 
   public boolean isLauncherUpToSpeed() {
-    return launcherControler.isAtSetpoint();
+    // Valid if velcity is within 10% of setpoint.
-  }
+    // May need to lower this
-
+    return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
-  public void turretControl(double position)
-  {
-      turretMotor.setControl(new PositionVoltage(position));
   }
 
   public void setTargetElevation(double elev)
@@ -113,97 +146,118 @@ public class TurretSubsystem extends TestableSubsystem {
     return this.runOnce(() -> setLauncher(-.5));
   }
 
-  public void startHoming()
+  public Command setShooter(int speedRPM){
-  {
+    return Commands.runEnd(() -> {
-    turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
+      setLauncher(speedRPM);
-    currentState = TurretState.HOMING;
+    }, () -> {
+      launcherMotor.stopMotor();
+    }, this);
   }
 
-  public void startRotate(double speed)
+  public void homeTurret()
   {
-    turretGoingPos=speed>0;
+    if(turretState != TurretState.INIT){
+      return; // Early return, we don't want to home if we already have
+    }
+    turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
+    turretState = TurretState.HOMING;
+  }
 
-    if(!passedLimit())
+  public boolean isTurretReady(){
+    return turretState == TurretState.READY;
+  }
+
+  public void turretRotateDir(double speed)
   {
     turretMotor.set(speed);
   }
-    else
+
+  public double getTurretAngleRobotRelative()
   {
-      turretMotor.set(0);
+    return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO;
-    }
   }
 
-  public boolean passedLimit(){
+  private double getTurretMotorPosition(){
-    return (getTurretAngle().gt(limitPos) && turretGoingPos) || (getTurretAngle().lt(limitNeg) && !turretGoingPos);
+    return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
-  }
-
-  public Angle getTurretAngle()
-  {
-    return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO);
   }
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
-    switch (currentState) {
-      case HOMING:
-        periodicHoming();
-        break;
-      case MANUAL:
-        periodicManual();
-        break;
 
-      default:
+    if(turretState == TurretState.HOMING)
-        break;
+    {
+      turretMotor.set(0.05);
+
+      if(turretLimitSwitch.get())
+      { // Turret has tripped the switch
+        turretMotor.stopMotor();
+        turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
+        turretState = TurretState.READY;
+        // Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset);
+        // Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset);
+        // Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
+        // Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
+        turretMotor.getConfigurator().apply(
+          new SoftwareLimitSwitchConfigs()
+            // .withForwardSoftLimitEnable(true)
+            // .withForwardSoftLimitThreshold(forwardLimit)
+            // .withReverseSoftLimitEnable(true)
+            // .withReverseSoftLimitThreshold(reverseLimit)
+        );
+        // setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees));
+        setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
+      }
     }
 
     if(LOGGING)
     {
-      Logger.recordOutput("Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
+      Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
+      Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
+      Logger.recordOutput("TurretSubsystem-TurretState", turretState);
+      Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative());
+      Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
+      Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
+      Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees));
+      Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
+      Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
+      Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
+      Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
     }
 
   }
 
-  private void periodicHoming()
+  public void setTurretFieldRelative(double pos)
   {
-    if(turretLimitSwitch.get())
+    if(turretState != TurretState.READY)
     {
-      currentState = TurretState.MANUAL;
+      // Not ready to set angle
-      turretMotor.set(0);
+      return;
-      turretMotor.setPosition(0);
-      goTo(limitPos);
     }
   }
 
-  private void goTo(Angle pos)
+  public void setTurretRobotRelative(double pos)
   {
-    if(pos.gt(limitNeg) && pos.lt(limitPos))
+    if(turretState != TurretState.READY)
     {
-      while(getTurretAngle()!=pos)
+      // Not ready to set angle
-    {
+      return;
-      if(getTurretAngle().gt(pos))
-      {
-        turretMotor.set(.5);
-      }
-      else
-      {
-        turretMotor.set(-.5);
-      }
-    }
-    }
     }
 
-  private void periodicManual()
+    setTurretMotorRotation(pos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
-  {   
-    if(passedLimit())
-    {
-      turretMotor.set(0);
   }
+
+  public void setTurretMotorRotation(double pos){
+    turretMotor.setControl(turretControlMode.withPosition((pos + turretOffsetInRotations)));
+    turretSim.setRawRotorPosition(pos + turretOffsetInRotations);
+  }
+
+  private double getRequestedPosTurretRelative() {
+    return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
   }
 
   @PostTest()
   public void TurretSubsystem_CANDevicesConnected(){
-    assertThat(launcherMotor.getDeviceId()).as("Launcher motor not connected!").isGreaterThan(0);
+    assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse();
     assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue();
   }