From 7f5ebf30de87ceaa2cd6b8fb7f48943f596974fe Mon Sep 17 00:00:00 2001
From: Niosocket11 <daniel.tolkachov@gmail.com>
Date: Sat, 14 Feb 2026 21:42:04 -0800
Subject: [PATCH 1/2] Add Shooter subsystem, shot calc, and path files

Add shooter support and pathplanner assets. Files added: Shooter subsystem (SparkFlex config, RPM control, manual mode), ShootOnFly command (skeleton to compute shot path using drivetrain pose/velocity), ShotCalculator (physics-based shot/rpm, clearance and feasibility checks). Constants.java: add Shooterc config constants (IDs, PID, gearReduction, launch angle). Drivetrain.java: reorganized imports, removed legacy/commented code, added getDrivetrainPosition(), getDrivetrainVelocity(), velocity estimation (updateVelocity) and call in periodic to provide robot velocities to shooting logic. Also add PathPlanner navgrid.json and an Example Path.path for autonomous path planning. These changes enable computing on-the-fly shooter setpoints and include initial pathplanner resources.
---
 src/main/deploy/pathplanner/navgrid.json      |   1 +
 .../pathplanner/paths/Example Path.path       |  54 ++++
 .../java/org/carlmontrobotics/Constants.java  |  11 +
 .../commands/ShooterCommands/ShootOnFly.java  |  67 ++++
 .../ShooterCommands/ShotCalculator.java       | 103 ++++++
 .../subsystems/Drivetrain.java                | 293 ++++++------------
 .../carlmontrobotics/subsystems/Shooter.java  |  89 ++++++
 7 files changed, 427 insertions(+), 191 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/navgrid.json
 create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path
 create mode 100644 src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShootOnFly.java
 create mode 100644 src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java
 create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Shooter.java

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..ac5f521
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 0000000..803210b
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.618,
+        "y": 4.038
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.618,
+        "y": 4.038
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 11.916,
+        "y": 4.038
+      },
+      "prevControl": {
+        "x": 10.916,
+        "y": 4.038
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
index 3a841ee..46021f0 100644
--- a/src/main/java/org/carlmontrobotics/Constants.java
+++ b/src/main/java/org/carlmontrobotics/Constants.java
@@ -268,5 +268,16 @@ public static class LimeLightc {
 		public static final int[] sampleLL1_VALID_IDS = {1, 2, 12, 13};
 		public static final int[] sampleLL2_VALID_IDS = {1, 6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22};
 	}
+
+	public static class Shooterc {
+		public static final int masterID = 1;
+		public static final boolean masterInverted = false;
+		public static final int followerID = 2;
+		public static final double kP = 2;
+		public static final double kI = 0;
+		public static final double kD = 0; 
+		public static final double gearReduction = 1;
+		public static final double launchAngleRad = Math.PI/4;
+	}
 }
 //#endregion
\ No newline at end of file
diff --git a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShootOnFly.java b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShootOnFly.java
new file mode 100644
index 0000000..a4d9d92
--- /dev/null
+++ b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShootOnFly.java
@@ -0,0 +1,67 @@
+// 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 org.carlmontrobotics.commands.ShooterCommands;
+
+import java.nio.channels.Pipe;
+
+import org.carlmontrobotics.subsystems.Drivetrain;
+import org.carlmontrobotics.subsystems.Shooter;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class ShootOnFly extends Command {
+  private Drivetrain dt;
+  private Shooter shooter;
+  private boolean isRed;
+  private Pose2d current2dPose;
+  private Pose3d current3dPose;
+  private Pose3d goalPose;
+  private double[] velocityVectors;
+  private double shooterVelocity;
+  private Translation3d path;
+
+  /** Creates a new ShootOnFly. */
+  public ShootOnFly(Drivetrain dt, Shooter shooter) {
+    addRequirements(this.dt = dt, this.shooter = shooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    isRed = DriverStation.getAlliance()
+      .map(a -> a == DriverStation.Alliance.Red)
+      .orElse(false);   // default when unknown
+    goalPose = isRed ? new Pose3d(11.916, 4.038, 1.8237877672, new Rotation3d()) : new Pose3d(4.618, 4.038, 1.8237877672, new Rotation3d());
+    current2dPose = dt.getDrivetrainPosition();
+    current3dPose = new Pose3d(current2dPose);
+    velocityVectors = dt.getDrivetrainVelocity();
+    shooterVelocity = shooter.getVelocity();
+    velocityVectors[0] += shooterVelocity*Math.cos(current2dPose.getRotation().getRadians());
+    velocityVectors[1] += shooterVelocity*Math.sin(current2dPose.getRotation().getRadians());
+    path = goalPose.getTranslation().minus(current3dPose.getTranslation());
+    
+    
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // 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;
+  }
+}
diff --git a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java
new file mode 100644
index 0000000..d1dba2e
--- /dev/null
+++ b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java
@@ -0,0 +1,103 @@
+package org.carlmontrobotics.commands.ShooterCommands;
+
+import static org.carlmontrobotics.Constants.Shooterc.*;
+
+public class ShotCalculator {
+
+    public static class ShotResult {
+        public double requiredRPM;
+        public boolean impossible;
+        public boolean nearImpossible;
+        public boolean descendingAtGoal;
+        public boolean clearanceOK;
+        public double[] clearanceMargins; // meters above each obstacle
+    }
+
+    public static ShotResult calculateShot(
+            double dx, double dy, double dz,          // displacement to goal (m)
+            double vxRobot, double vyRobot,           // robot velocity field-relative (m/s)
+            double wheelRadiusMeters,                 // flywheel radius
+            double[] obstacleDistances,               // horizontal distance to each obstacle
+            double[] obstacleHeights,                 // required height at each obstacle
+            double clearanceSafety                    // extra margin (m)
+    ) {
+
+        ShotResult result = new ShotResult();
+        double g = 9.81;
+
+        double x = Math.hypot(dx, dy);
+        double cos = Math.cos(launchAngleRad);
+        double tan = Math.tan(launchAngleRad);
+
+        double margin = x * tan - dz;
+
+        // 1. impossible check
+        if (margin <= 0.0) {
+            result.impossible = true;
+            result.requiredRPM = Double.POSITIVE_INFINITY;
+            return result;
+        }
+
+        // 2. field-relative projectile speed
+        double vField = Math.sqrt(
+            (g * x * x) /
+            (2.0 * cos * cos * margin)
+        );
+
+        // 3. compensate robot motion
+        double dirX = dx / x;
+        double dirY = dy / x;
+
+        double vxField = vField * dirX;
+        double vyField = vField * dirY;
+
+        double vxExit = vxField - vxRobot;
+        double vyExit = vyField - vyRobot;
+
+        double exitSpeed = Math.hypot(vxExit, vyExit);
+
+        // 4. convert to RPM
+        double wheelRadPerSec = exitSpeed / wheelRadiusMeters;
+        result.requiredRPM = wheelRadPerSec * 60.0 / (2.0 * Math.PI);
+
+        // 5. near-impossible check
+        result.nearImpossible = margin < 0.20;
+
+        // 6. descending check
+        double vzFinal =
+            vField * Math.sin(launchAngleRad)
+            - g * x / (vField * cos);
+
+        result.descendingAtGoal = vzFinal < 0.0;
+
+        // 7. clearance checks
+        if (obstacleDistances != null && obstacleHeights != null) {
+
+            int n = obstacleDistances.length;
+            result.clearanceMargins = new double[n];
+            result.clearanceOK = true;
+
+            for (int i = 0; i < n; i++) {
+
+                double s = obstacleDistances[i];
+
+                double z =
+                    s * Math.tan(launchAngleRad)
+                    - (g * s * s) /
+                      (2.0 * vField * vField * cos * cos);
+
+                double marginClear = z - obstacleHeights[i];
+                result.clearanceMargins[i] = marginClear;
+
+                if (marginClear < clearanceSafety) {
+                    result.clearanceOK = false;
+                }
+            }
+        } else {
+            result.clearanceOK = true;
+            result.clearanceMargins = new double[0];
+        }
+
+        return result;
+    }
+}
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java
index 404994a..dbe0ca2 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java
@@ -1,9 +1,40 @@
 package org.carlmontrobotics.subsystems;
 
+import static edu.wpi.first.units.Units.Meter;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Rotation;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volt;
+import static edu.wpi.first.units.Units.Volts;
+import static org.carlmontrobotics.Config.CONFIG;
+import static org.carlmontrobotics.Constants.Drivetrainc.COLLISION_ACCELERATION_THRESHOLD;
+import static org.carlmontrobotics.Constants.Drivetrainc.driveBackLeftPort;
+import static org.carlmontrobotics.Constants.Drivetrainc.driveBackRightPort;
+import static org.carlmontrobotics.Constants.Drivetrainc.driveFrontLeftPort;
+import static org.carlmontrobotics.Constants.Drivetrainc.driveFrontRightPort;
+import static org.carlmontrobotics.Constants.Drivetrainc.driveGearing;
+import static org.carlmontrobotics.Constants.Drivetrainc.isGyroReversed;
+import static org.carlmontrobotics.Constants.Drivetrainc.maxSpeed;
+import static org.carlmontrobotics.Constants.Drivetrainc.secsPer12Volts;
+import static org.carlmontrobotics.Constants.Drivetrainc.trackWidth;
+import static org.carlmontrobotics.Constants.Drivetrainc.turnBackLeftPort;
+import static org.carlmontrobotics.Constants.Drivetrainc.turnBackRightPort;
+import static org.carlmontrobotics.Constants.Drivetrainc.turnFrontLeftPort;
+import static org.carlmontrobotics.Constants.Drivetrainc.turnFrontRightPort;
+import static org.carlmontrobotics.Constants.Drivetrainc.turnGearing;
+import static org.carlmontrobotics.Constants.Drivetrainc.wheelBase;
+import static org.carlmontrobotics.Constants.Drivetrainc.wheelDiameterMeters;
+
 import java.util.Arrays;
 import java.util.Map;
 import java.util.function.Supplier;
 
+//Constants
+import org.carlmontrobotics.Constants;
+import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle;
+import org.carlmontrobotics.commands.DriveCommands.TeleopDrive;
 //lib199
 import org.carlmontrobotics.lib199.MotorConfig;
 import org.carlmontrobotics.lib199.MotorControllerFactory;
@@ -11,22 +42,52 @@
 import org.carlmontrobotics.lib199.swerve.SwerveModule;
 import org.carlmontrobotics.lib199.swerve.SwerveModuleSim;
 
-import static org.carlmontrobotics.Config.CONFIG;
+import com.ctre.phoenix6.hardware.CANcoder;
+//pathplanner
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.util.PathPlannerLogging;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.hal.SimDouble;
-
+//math
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.units.measure.MutAngle;
+import edu.wpi.first.units.measure.MutAngularVelocity;
+import edu.wpi.first.units.measure.MutDistance;
+import edu.wpi.first.units.measure.MutLinearVelocity;
+import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
-
 //wpilib
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
@@ -37,85 +98,6 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
-//math
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.math.controller.PIDController;
-
-//vendordeps
-import com.ctre.phoenix6.SignalLogger;
-import com.ctre.phoenix6.hardware.CANcoder;
-import com.studica.frc.AHRS;
-import com.studica.frc.AHRS.NavXComType;
-
-//pathplanner
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
-import com.pathplanner.lib.util.PathPlannerLogging;
-import com.pathplanner.lib.config.PIDConstants;
-import com.pathplanner.lib.config.RobotConfig;
-
-//rev
-import com.revrobotics.spark.ClosedLoopSlot;
-import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkFlex;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.PersistMode;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkBaseConfig;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.ClosedLoopConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-
-//units
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.units.measure.LinearVelocity;
-import edu.wpi.first.units.measure.MutAngle;
-import edu.wpi.first.units.measure.MutAngularVelocity;
-import edu.wpi.first.units.measure.MutDistance;
-import edu.wpi.first.units.measure.MutLinearVelocity;
-import edu.wpi.first.units.measure.MutVoltage;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.MutableMeasure;
-import edu.wpi.first.units.measure.Velocity;
-import edu.wpi.first.units.measure.Voltage;
-
-import static edu.wpi.first.units.Units.Volts;
-import static edu.wpi.first.units.Units.MetersPerSecond;
-import static edu.wpi.first.units.Units.Rotation;
-import static edu.wpi.first.units.Units.RotationsPerSecond;
-import static edu.wpi.first.units.Units.Seconds;
-import static edu.wpi.first.units.Units.Volt;
-import static edu.wpi.first.units.Units.Meter;
-import static edu.wpi.first.units.Units.Meters;
-
-//Constants
-import org.carlmontrobotics.Constants;
-import org.carlmontrobotics.Constants.Drivetrainc;
-import org.carlmontrobotics.Constants.Drivetrainc.Autoc;
-import org.carlmontrobotics.Robot;
-import org.carlmontrobotics.subsystems.Limelight;
-import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle;
-import org.carlmontrobotics.commands.DriveCommands.TeleopDrive;
-import static org.carlmontrobotics.Constants.Drivetrainc.*;
-import static org.carlmontrobotics.Constants.LimeLightc.*;
-
 public class Drivetrain extends SubsystemBase {
 
     private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI);
@@ -415,123 +397,17 @@ public void setDrivingIdleMode(boolean brake) {
 
     @Override
     public void periodic() {
-        detectCollision(); //This does nothing
         PathPlannerLogging.logCurrentPose(getPose());
-
-        //maybe add the field with the position of the robot with only limelight and the field with the position of the robot with only odometry?
-        //We can compare the two fields to see if odometry is causing the pose to be inaccurate when it hits the reef.
-
-        // SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble());
-        // SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition());
-        // double goal = SmartDashboard.getNumber("GoalPos", 0);
-        // PIDController pid = new PIDController(kP, kI, kD);
-        // kP = SmartDashboard.getNumber("kP", 0);
-        // kI = SmartDashboard.getNumber("kI", 0);
-        // kD = SmartDashboard.getNumber("kD", 0);
-        //pid.setIZone(20);
-        //SmartDashboard.putBoolean("atgoal", pid.atSetpoint());
-        // SparkMaxConfig config = new SparkMaxConfig();
-        
-        //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder);
-        // System.out.println(kP);
-        // config.closedLoop.pid(kP ,kI,kD);
-        // config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing);
-        // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
-        // //moduleFL.move(0.0000001, 180);
-        //moduleFL.move(0.01, 180);
-        // moduleFR.move(0.000000001, 0);
-        // moduleBR.move(0.0000001, 0);
-        // moduleFL.move(0.000001, 0);
-        // moduleBL.move(0.000001, 0);
-        // turnPidControllers[0].setReference(goal
-
-        // , ControlType.kPosition, ClosedLoopSlot.kSlot0);
-        
-        
-        // 167 -> -200
-        // 138 -> 360
-        // for (CANcoder coder : turnEncoders) {
-        //     SignalLogger.writeDouble("Regular position " + coder.toString(),
-        //     coder.getPosition().getValue().baseUnitMagnitude());
-        //     SignalLogger.writeDouble("Velocity " + coder.toString(),
-        //     coder.getVelocity().getValue().baseUnitMagnitude());
-        //     SignalLogger.writeDouble("Absolute position " + coder.toString(),
-        //     coder.getAbsolutePosition().getValue().baseUnitMagnitude());
-        // }
-        // String out=""; int i=0;
-        // for (CANcoder coder : turnEncoders) {
-        //     out+=String.format("[i] Abs Pos: %.3f Goal Pos: %.3f ", coder.getAbsolutePosition().getValue().baseUnitMagnitude(),0);
-        //     i++;
-        // }
-        // lobotomized to prevent ucontrollabe swerve behavior
-        // turnMotors[2].setVoltage(SmartDashboard.getNumber("kS", 0));
-        // moduleFL.periodic();
-        // moduleFR.periodic();
-        // moduleBL.periodic();
-        // moduleBR.periodic();
-        double goal = SmartDashboard.getNumber("bigoal", 0);
         for (SwerveModule module : modules) {
           // module.turnPeriodic();
           // module.turnPeriodic();
-          module.move(0.00000000001, goal);
           module.periodic();
         }
-
-        // field.setRobotPose(odometry.getPoseMeters());
-
-        
-
-        // odometry.update(gyro.getRotation2d(), getModulePositions());
-
-        // poseEstimator.update(gyro.getRotation2d(), getModulePositions());
-        
-        //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions());
-
-        // updateMT2PoseEstimator();
-
-        // double currSetX =
-        // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX);
-        // double currSetY =
-        // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY);
-        // double currSetTheta = SmartDashboard
-        // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta);
-
-        // if (lastSetX != currSetX || lastSetY != currSetY
-        // || lastSetTheta != currSetTheta) {
-        // setPose(new Pose2d(currSetX, currSetY,
-        // Rotation2d.fromDegrees(currSetTheta)));
-        // }
-
-        // setPose(new Pose2d(getPose().getTranslation().getX(),
-        // getPose().getTranslation().getY(),
-        // Rotation2d.fromDegrees(getHeading())));
-
-
-        // SmartDashboard.putNumber("X position with limelight", getPoseWithLimelight().getX());
-        // SmartDashboard.putNumber("Y position with limelight", getPoseWithLimelight().getY());
-        SmartDashboard.putNumber("X position with gyro", getPose().getX());
-        SmartDashboard.putNumber("Y position with gyro", getPose().getY());
-        SmartDashboard.putData(CONFIG);
-        
-        //For finding acceleration of drivetrain for collision detector
-        SmartDashboard.putNumber("Accel X", accelX);
-        SmartDashboard.putNumber("Accel Y", accelY);
-        SmartDashboard.putNumber("2D Acceleration ", accelXY);
-
-        // // // SmartDashboard.putNumber("Pitch", gyro.getPitch());
-        // // // SmartDashboard.putNumber("Roll", gyro.getRoll());
-        // SmartDashboard.putNumber("Raw gyro angle", gyro.getAngle());
-        // SmartDashboard.putNumber("Robot Heading", getHeading());
-        // // // SmartDashboard.putNumber("AdjRoll", gyro.getPitch() - initPitch);
-        // // // SmartDashboard.putNumber("AdjPitch", gyro.getRoll() - initRoll);
-        // SmartDashboard.putBoolean("Field Oriented", fieldOriented);
-        // SmartDashboard.putNumber("Gyro Compass Heading", gyro.getCompassHeading());
-        // SmartDashboard.putNumber("Compass Offset", compassOffset);
-        // SmartDashboard.putBoolean("Current Magnetic Field Disturbance", gyro.isMagneticDisturbance());
         SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle());
         SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle());
         SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle());
         SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle());
+        updateVelocity();
     }
 
     @Override
@@ -1328,4 +1204,39 @@ public void keepRotateMotorsAtDegrees(int angle) {
     public double getGyroRate() {
         return gyro.getRate();
     }
+
+    public Pose2d getDrivetrainPosition() {
+        return poseEstimator.getEstimatedPosition();
+    }
+
+    public double[] getDrivetrainVelocity() {
+        double [] info = {drivetrainVX, drivetrainVY, drivetrainVR};
+        return info;
+    }
+
+    Pose2d lastPose;
+    double lastTime;
+    /** X direction velocity: m/s*/
+    private double drivetrainVX;
+    /** Y direction velocity: m/s */
+    private double drivetrainVY;
+    /** Rotation velocity: rad/s */
+    private double drivetrainVR;
+
+    private void updateVelocity() {
+        Pose2d currentPose = poseEstimator.getEstimatedPosition();
+        double now = Timer.getFPGATimestamp();
+
+        double dt = now - lastTime;
+        Translation2d delta = currentPose.getTranslation().minus(lastPose.getTranslation());
+
+        drivetrainVX = delta.getX() / dt;
+        drivetrainVY = delta.getY() / dt;
+
+        drivetrainVR =
+            currentPose.getRotation().minus(lastPose.getRotation()).getRadians() / dt;
+
+        lastPose = currentPose;
+        lastTime = now;
+    }
 }
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Shooter.java b/src/main/java/org/carlmontrobotics/subsystems/Shooter.java
new file mode 100644
index 0000000..4b0472d
--- /dev/null
+++ b/src/main/java/org/carlmontrobotics/subsystems/Shooter.java
@@ -0,0 +1,89 @@
+// 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 org.carlmontrobotics.subsystems;
+
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.config.ClosedLoopConfig;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static org.carlmontrobotics.Constants.Shooterc.*;
+
+import org.carlmontrobotics.lib199.MotorConfig;
+import org.carlmontrobotics.lib199.MotorControllerFactory;
+import org.carlmontrobotics.lib199.MotorControllerType;
+
+public class Shooter extends SubsystemBase {
+  /** Creates a new Shooter. */
+  private SparkFlex masterShooter;
+  private SparkFlex followShooter;
+  private RelativeEncoder masterEncoder;
+  private RelativeEncoder followEncoder;
+  private SparkBaseConfig masterConfig;
+  private SparkBaseConfig followConfig;
+  private SparkClosedLoopController pidController;
+  private double shooterGoal;
+  private boolean manualMode = false;
+
+  public Shooter() {
+    configureMotors();
+    masterShooter = MotorControllerFactory.createSparkFlex(masterID, MotorConfig.NEO_VORTEX, masterConfig);
+    followShooter = MotorControllerFactory.createSparkFlex(followerID, MotorConfig.NEO_VORTEX, followConfig);
+  }
+
+  private void configureMotors() {
+    masterConfig = MotorControllerFactory.createConfig(MotorControllerType.SPARK_FLEX);
+    masterConfig.idleMode(IdleMode.kCoast)
+                .encoder
+                  .positionConversionFactor(gearReduction) //rotations
+                  .velocityConversionFactor(gearReduction); //rotations per minute
+    
+    followConfig.apply(masterConfig);
+    followConfig.follow(masterID, true);
+
+    masterConfig.inverted(masterInverted)
+                .closedLoop.pid(kP, kI, kD);
+  }
+
+  public void setVoltage(double voltage) {
+    masterShooter.setVoltage(voltage);
+  }
+
+  public void setVoltagePercentage(double percentage) {
+    masterShooter.set(percentage);
+  }
+
+  public void setRPMGoal(double goal) {
+    shooterGoal = goal;
+    pidController.setSetpoint(goal, ControlType.kVelocity);
+  }
+
+  public void turnOnManualMode() {
+    if (!manualMode) {
+      manualMode = true;
+      pidController.setSetpoint(0, ControlType.kDutyCycle);
+    }
+  }
+
+  public double getRPMGoal() {
+    return shooterGoal;
+  }
+
+  /**Gets RPM */
+  public double getVelocity() {
+    return masterEncoder.getVelocity();
+  }
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

From 2bb44569303f1e97dcf3d361cca4abf27014f01e Mon Sep 17 00:00:00 2001
From: Niosocket11 <daniel.tolkachov@gmail.com>
Date: Wed, 18 Feb 2026 13:22:24 -0800
Subject: [PATCH 2/2] Add shooter auto-align and shot calculations

Add support for shooting-on-the-fly: integrate Shooter into TeleopDrive and wire a POV-based shoot/reset binding in RobotContainer. Introduce HeadingAlignController and HexClosest utilities and extend ShotCalculator (signature change + safety constant) so TeleopDrive can compute heading and required shot velocity while moving. Update Constants with shooter parameters, goal poses and button mappings (placeholder TODOs). Adjust Shooter API to use pidController setpoints and add stop(); add a minimal Indexer subsystem. Remove obsolete RotateToNearest60 text file and apply minor path coordinate tweaks.
---
 .../pathplanner/paths/Example Path.path       |   4 +-
 .../java/org/carlmontrobotics/Constants.java  |  30 +++-
 .../org/carlmontrobotics/RobotContainer.java  |  12 +-
 .../DriveCommands/RotateToNearest60.txt       |  49 ------
 .../commands/DriveCommands/TeleopDrive.java   | 142 +++++++++++-------
 .../HeadingAlignController.java               |  53 +++++++
 .../commands/ShooterCommands/HexClosest.java  |  75 +++++++++
 .../ShooterCommands/ShotCalculator.java       |   5 +-
 .../carlmontrobotics/subsystems/Indexer.java  |  17 +++
 .../carlmontrobotics/subsystems/Shooter.java  |   8 +-
 10 files changed, 271 insertions(+), 124 deletions(-)
 delete mode 100644 src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt
 create mode 100644 src/main/java/org/carlmontrobotics/commands/ShooterCommands/HeadingAlignController.java
 create mode 100644 src/main/java/org/carlmontrobotics/commands/ShooterCommands/HexClosest.java
 create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Indexer.java

diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
index 803210b..b04525a 100644
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 4.618,
+        "x": 4.640294126157407,
         "y": 4.038
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.618,
+        "x": 5.640294126157407,
         "y": 4.038
       },
       "isLocked": false,
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
index 46021f0..62b5823 100644
--- a/src/main/java/org/carlmontrobotics/Constants.java
+++ b/src/main/java/org/carlmontrobotics/Constants.java
@@ -10,11 +10,14 @@
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.path.PathConstraints;
 
+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.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.XboxController.Axis;
 import edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj2.command.button.POVButton;
 import edu.wpi.first.math.util.Units;
 import static org.carlmontrobotics.Config.CONFIG;
 
@@ -38,7 +41,8 @@ public static final class Driver {
 			public static final int port = 0;
 
 			public static final int slowDriveButton = Button.kLeftBumper.value;
-			public static final int resetFieldOrientationButton = Button.kRightBumper.value;
+			public static final int shootOnFlyButton = Button.kRightBumper.value;
+			public static final int resetFieldOrientationButton = 180; //down dpad
 			public static final Axis RIGHT_TRIGGER_BUTTON = Axis.kRightTrigger;
 			public static final Axis LEFT_TRIGGER_BUTTON = Axis.kLeftTrigger;
 
@@ -270,14 +274,24 @@ public static class LimeLightc {
 	}
 
 	public static class Shooterc {
-		public static final int masterID = 1;
-		public static final boolean masterInverted = false;
-		public static final int followerID = 2;
-		public static final double kP = 2;
-		public static final double kI = 0;
-		public static final double kD = 0; 
+		public static final int masterID = 1; //TODO
+		public static final boolean masterInverted = false; //TODO
+		public static final int followerID = 2; //TODO
+		public static final double kP = 2; //TODO
+		public static final double kI = 0; //TODO
+		public static final double kD = 0; //TODO
 		public static final double gearReduction = 1;
-		public static final double launchAngleRad = Math.PI/4;
+		public static final double launchAngleRad = Math.PI/4; //TODO figure out real value
+		public static final double wheelRadiusMeters = 0.3; //TODO figure out real value
+		public static final double passiveVelocity = 100; //TODO
+		public static final double thetaAlignP = 1; //TODO
+		public static final double toleranceRad = Math.PI/90; //Looks right
+
+		//Poses
+		public static final Pose2d centerOfRedGoal2d = new Pose2d();
+		public static final Pose2d centerOfBlueGoal2d = new Pose2d();
+		public static final Pose3d centerOfRedGoal3d = new Pose3d();
+		public static final Pose3d centerOfBlueGoal3d = new Pose3d();
 	}
 }
 //#endregion
\ No newline at end of file
diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
index b921a51..55b03d4 100644
--- a/src/main/java/org/carlmontrobotics/RobotContainer.java
+++ b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -73,6 +73,7 @@ public class RobotContainer {
 
     public final Limelight limelight = new Limelight();
     public final Drivetrain drivetrain =  new Drivetrain(limelight);
+    public final Shooter shooter = new Shooter();
 
 
     private SendableChooser<Command> autoChooser = new SendableChooser<>();   
@@ -86,16 +87,12 @@ public RobotContainer() {
 
         SmartDashboard.putData("Auto Chooser", autoChooser); 
 
-        SmartDashboard.putBoolean("AlignOverride", alignOverride);
-        SmartDashboard.putBoolean("AutoScoring", autoScoring);
-        SmartDashboard.putBoolean("AlignOverride", true);
         //#endregion
         setDefaultCommands();
         setBindingsDriver();
         setBindingsManipulator();
 
         SmartDashboard.putBoolean("Baby Mode", Config.CONFIG.isBabyMode());
-        // SmartDashboard.putData("Rotate Command",new RotateToTag(drivetrain, limelight));
         SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString());
         SmartDashboard.putString("Location", DriverStation.getLocation().toString());
         SmartDashboard.putBoolean("Connected to FMS?", DriverStation.isFMSAttached());
@@ -105,7 +102,7 @@ public RobotContainer() {
    
     //#region ButtonBindings
     private void setBindingsDriver() {
-        new JoystickButton(driverController, Driver.resetFieldOrientationButton)
+        new POVButton(driverController, Driver.resetFieldOrientationButton)
             .onTrue(new InstantCommand(drivetrain::resetFieldOrientation));
         axisTrigger(driverController, Driver.RIGHT_TRIGGER_BUTTON, 0.2)
             .onTrue(new InstantCommand(()->drivetrain.setFieldOriented(false)))
@@ -127,12 +124,13 @@ private void RegisterCustomAutos(){}
   private void setDefaultCommands() {
     drivetrain.setDefaultCommand(new TeleopDrive(
       drivetrain,
+      shooter,
       () -> ProcessedAxisValue(driverController, Axis.kLeftY),//.06 drift purple, .10 drift black
       () -> ProcessedAxisValue(driverController, Axis.kLeftX),
       () -> ProcessedAxisValue(driverController, Axis.kRightX),
       () -> driverController.getRawButton(OI.Driver.slowDriveButton),
-      manipulatorController,
-      () -> SmartDashboard.getBoolean("Baby Mode", Config.CONFIG.isBabyMode())
+      () -> SmartDashboard.getBoolean("Baby Mode", Config.CONFIG.isBabyMode()),
+      () -> driverController.getRawButton(OI.Driver.shootOnFlyButton)
       ));
   }
   //#endregion
diff --git a/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt b/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt
deleted file mode 100644
index 9d2a4c9..0000000
--- a/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt
+++ /dev/null
@@ -1,49 +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 org.carlmontrobotics.commands.DriveCommands;
-
-import org.carlmontrobotics.subsystems.Drivetrain;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-
-public class RotateToNearest60 extends Command {
-    
-    private final RotateToFieldRelativeAngle rotateCommand;
-    private final Drivetrain drivetrain;
-    
-    public RotateToNearest60(Drivetrain drivetrain) {
-        this.drivetrain = drivetrain;
-        Rotation2d targetAngle = getNearest60(drivetrain.getHeading());
-        this.rotateCommand = new RotateToFieldRelativeAngle(targetAngle, drivetrain);
-        
-        addRequirements(drivetrain);
-    }
-    
-    @Override
-    public void initialize() {
-        rotateCommand.initialize();
-    }
-    
-    @Override
-    public void execute() {
-        rotateCommand.execute();
-    }
-    
-    @Override
-    public boolean isFinished() {
-        return rotateCommand.isFinished();
-    }
-    
-    @Override
-    public void end(boolean interrupted) {
-        rotateCommand.end(interrupted);
-    }
-    
-    private Rotation2d getNearest60(double currentAngle) {
-        double modAngle = (Math.round(currentAngle / 60.0) * 60) % 360;
-        if (modAngle > 180) modAngle -= 360; // Keep angle within -180 to 180
-        return Rotation2d.fromDegrees(modAngle);
-    }
-}
diff --git a/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java
index 0386341..8e544e0 100644
--- a/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java
+++ b/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java
@@ -1,24 +1,37 @@
 package org.carlmontrobotics.commands.DriveCommands;
 
 import static org.carlmontrobotics.Constants.Drivetrainc.*;
+import static org.carlmontrobotics.Constants.Shooterc.centerOfBlueGoal2d;
+import static org.carlmontrobotics.Constants.Shooterc.centerOfRedGoal2d;
+import static org.carlmontrobotics.Constants.Shooterc.passiveVelocity;
 
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import org.carlmontrobotics.Constants;
 import org.carlmontrobotics.Robot;
+import org.carlmontrobotics.commands.ShooterCommands.HeadingAlignController;
+import org.carlmontrobotics.commands.ShooterCommands.HexClosest;
+import org.carlmontrobotics.commands.ShooterCommands.ShotCalculator;
 import org.carlmontrobotics.subsystems.Drivetrain;
+import org.carlmontrobotics.subsystems.Shooter;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class TeleopDrive extends Command {
 
   private static double robotPeriod = Robot.kDefaultPeriod;
-  private final Drivetrain drivetrain;
+  private final Drivetrain dt;
+  private final Shooter shooter;
   private DoubleSupplier fwd;
   private DoubleSupplier str;
   private DoubleSupplier rcw;
@@ -26,21 +39,23 @@ public class TeleopDrive extends Command {
   private double currentForwardVel = 0;
   private double currentStrafeVel = 0;
   private double prevTimestamp;
-  GenericHID manipulatorController;
-  BooleanSupplier babyModeSupplier;
+  private BooleanSupplier babyModeSupplier;
+  private BooleanSupplier shootOnFly;
+
+  private boolean isRed = false; 
 
   /**
    * Creates a new TeleopDrive.
    */
-  public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw,
-      BooleanSupplier slow, GenericHID manipulatorController, BooleanSupplier babyModeSupplier) {
-    addRequirements(this.drivetrain = drivetrain);
+  public TeleopDrive(Drivetrain drivetrain, Shooter shooter, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw,
+      BooleanSupplier slow, BooleanSupplier babyModeSupplier, BooleanSupplier shootOnFly) {
+    addRequirements(this.dt = drivetrain, this.shooter = shooter);
     this.fwd = fwd;
     this.str = str;
     this.rcw = rcw;
     this.slow = slow;
-    this.manipulatorController = manipulatorController;
     this.babyModeSupplier = babyModeSupplier;
+    this.shootOnFly = shootOnFly;
   }
 
   // Called when the command is initially scheduled.
@@ -51,6 +66,9 @@ public void initialize() {
     // SmartDashboard.putNumber("normal turn const", kNormalDriveRotation);
     // SmartDashboard.putNumber("normal speed const", kNormalDriveSpeed);
     prevTimestamp = Timer.getFPGATimestamp();
+    isRed = DriverStation.getAlliance()
+      .map(a -> a == DriverStation.Alliance.Red)
+      .orElse(false);   // default when unknown
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -58,22 +76,32 @@ public void initialize() {
   public void execute() {
     double currentTime = Timer.getFPGATimestamp();
     robotPeriod = currentTime - prevTimestamp;
-    if (!hasDriverInput()) {
-      drivetrain.drive(0,0,0);
-    }
-    else {
     double[] speeds = getRequestedSpeeds();
-    // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp);
     prevTimestamp = currentTime;
-    // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation);
-    // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed);
-    // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation);
-    // kNormalDriveSpeed = SmartDashboard.getNumber("normal speed const", kNormalDriveSpeed);
-
-    // SmartDashboard.putNumber("fwd", speeds[0]);
-    // SmartDashboard.putNumber("strafe", speeds[1]);
-    // SmartDashboard.putNumber("turn", speeds[2]);
-      drivetrain.drive(speeds[0], speeds[1], speeds[2]);
+    if (shootOnFly.getAsBoolean()) {
+      Pose2d currentPose2d = dt.getDrivetrainPosition();
+      double rotation = calculateRotationToAlign(currentPose2d);
+      boolean alignedForShot = calculateAlignmentValid(currentPose2d);
+      if (alignedForShot) {
+        ShotCalculator.ShotResult shotResult = calculateShotVelocity();
+        if (shotResult.impossible) {
+          shooter.setRPMGoal(passiveVelocity);
+          dt.drive(speeds[0], speeds[1], rotation);     
+        }
+        else {
+          double shotVelocity = shotResult.requiredRPM;
+        shooter.setRPMGoal(shotVelocity);
+        dt.drive(speeds[0], speeds[1], rotation);     
+        } 
+      }
+      else {
+        shooter.setRPMGoal(passiveVelocity);
+        dt.drive(speeds[0], speeds[1], rotation);     
+      }
+    }
+    else {
+      shooter.stop();
+      dt.drive(speeds[0], speeds[1], speeds[2]);
     }
   }
 
@@ -85,49 +113,21 @@ public double[] getRequestedSpeeds() {
     double forward = fwd.getAsDouble();
     double strafe = str.getAsDouble();
     double rotateClockwise = rcw.getAsDouble();
-    // SmartDashboard.putNumber("fwdIN", forward);
-    // SmartDashboard.putNumber("strafeIN", strafe);
-    // SmartDashboard.putNumber("turnIN", rotateClockwise);
-    // System.out.println("fwd str rcw: "+forward+", "+strafe+", "+rotateClockwise);
-    boolean slow2 = slow.getAsBoolean();
+    //boolean slow2 = slow.getAsBoolean(); gets not used ig
     forward *= maxForward;
     strafe *= maxStrafe;
     rotateClockwise *= maxRCW;
-
-    // System.out.println("teleopDrive ExtraSpeedMult%: "+drivetrain.extraSpeedMult);
     double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed);
-    double rotationMultiplier = drivetrain.extraSpeedMult + (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation);
-    // double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed);
-    // double rotationMultiplier = (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation);
-    
+    double rotationMultiplier = dt.extraSpeedMult + (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation);
     if(babyModeSupplier.getAsBoolean()){
       driveMultiplier = kBabyDriveSpeed; 
       rotationMultiplier = kBabyDriveRotation;
     }
-    // double driveMultiplier = kNormalDriveSpeed;
-    // double rotationMultiplier = kNormalDriveRotation;
-
     forward *= driveMultiplier;
     strafe *= driveMultiplier;
     rotateClockwise *= rotationMultiplier;
-
-    // Limit acceleration of the robot
-    // double accelerationX = (forward - currentForwardVel) / robotPeriod;
-    // double accelerationY = (strafe - currentStrafeVel) / robotPeriod;
-    // double translationalAcceleration = Math.hypot(accelerationX, accelerationY);
-    // SmartDashboard.putNumber("Translational Acceleration", translationalAcceleration);
-    // if (translationalAcceleration > autoMaxAccelMps2 && false) {//DOES NOT RUN!!
-    //   Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2,
-    //       Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX)));
-    //   Translation2d limitedVelocityVector = limitedAccelerationVector.times(robotPeriod);
-    //   currentForwardVel += limitedVelocityVector.getX();
-    //   currentStrafeVel += limitedVelocityVector.getY();
-    // } else {
     currentForwardVel = forward;
     currentStrafeVel = strafe;
-    // }
-    // SmartDashboard.putNumber("current velocity", Math.hypot(currentForwardVel, currentStrafeVel));
-
     return new double[] { currentForwardVel, currentStrafeVel, -rotateClockwise };
   }
 
@@ -137,6 +137,42 @@ public boolean hasDriverInput() {
         || MathUtil.applyDeadband(rcw.getAsDouble(), Constants.OI.JOY_THRESH)!=0;
   }
 
+
+  private double calculateRotationToAlign(Pose2d currentPose2d) {
+    if (isRed) {
+        return HeadingAlignController.calculateOmega(currentPose2d, centerOfRedGoal2d);
+    }
+    else {
+        return HeadingAlignController.calculateOmega(currentPose2d, centerOfBlueGoal2d);
+    }
+  }
+
+  private boolean calculateAlignmentValid(Pose2d currentPose2d) {
+    if (isRed) {
+        return HeadingAlignController.atGoal(currentPose2d, centerOfRedGoal2d);
+    }
+    else {
+        return HeadingAlignController.atGoal(currentPose2d, centerOfBlueGoal2d);
+    }
+  }
+
+  private ShotCalculator.ShotResult calculateShotVelocity() {
+    Pose3d goalPose = isRed ? new Pose3d(11.916, 4.038, 1.8237877672, new Rotation3d()) : new Pose3d(4.618, 4.038, 1.8237877672, new Rotation3d());
+    Pose2d current2dPose = dt.getDrivetrainPosition();
+    Pose3d current3dPose = new Pose3d(current2dPose);
+    double[] velocityVectors = dt.getDrivetrainVelocity();
+    double shooterVelocity = shooter.getVelocity();
+    velocityVectors[0] += shooterVelocity * Math.cos(current2dPose.getRotation().getRadians());
+    velocityVectors[1] += shooterVelocity * Math.sin(current2dPose.getRotation().getRadians());
+    Translation3d path = goalPose.getTranslation().minus(current3dPose.getTranslation());
+    Translation2d closestObstacle = HexClosest.closestVectorToHex(current2dPose, isRed);
+    double[] obstacleDistances = {closestObstacle.getX(), closestObstacle.getY()};
+    double[] obstacleHeights = {Units.inchesToMeters(72), Units.inchesToMeters(72)};
+    ShotCalculator.ShotResult speed = ShotCalculator.calculateShot(path.getX(), path.getY(), path.getZ(), velocityVectors[0], velocityVectors[1], obstacleDistances, obstacleHeights);
+    
+    return speed;
+  }
+
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
diff --git a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HeadingAlignController.java b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HeadingAlignController.java
new file mode 100644
index 0000000..e10b20a
--- /dev/null
+++ b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HeadingAlignController.java
@@ -0,0 +1,53 @@
+package org.carlmontrobotics.commands.ShooterCommands;
+
+import static org.carlmontrobotics.Constants.Shooterc.thetaAlignP;
+import static org.carlmontrobotics.Constants.Shooterc.toleranceRad;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public class HeadingAlignController {
+
+    /**
+     * Returns rotational speed (rad/s scaled by kP) needed to face target.
+     *
+     * @param currentPose robot pose (with rotation)
+     * @param targetPose pose representing the point to face (rotation ignored)
+     */
+    public static double calculateOmega(Pose2d currentPose, Pose2d targetPose) {
+        double dx = targetPose.getX() - currentPose.getX();
+        double dy = targetPose.getY() - currentPose.getY();
+
+        double desiredAngle = Math.atan2(dy, dx);
+        double currentAngle = currentPose.getRotation().getRadians();
+
+        double error = wrapAngle(desiredAngle - currentAngle);
+
+        return thetaAlignP * error;
+    }
+
+    /**
+     * Returns true if robot is aligned within tolerance.
+     */
+    public static boolean atGoal(Pose2d currentPose, Pose2d targetPose) {
+        double dx = targetPose.getX() - currentPose.getX();
+        double dy = targetPose.getY() - currentPose.getY();
+
+        double desiredAngle = Math.atan2(dy, dx);
+        double currentAngle = currentPose.getRotation().getRadians();
+
+        double error = wrapAngle(desiredAngle - currentAngle);
+
+        return Math.abs(error) <= toleranceRad;
+    }
+
+    /**
+     * Wrap angle to [-pi, pi]
+     */
+    private static double wrapAngle(double angle) {
+        angle %= Math.PI;
+        if (angle > Math.PI) {
+            angle -= 2.0 * Math.PI;
+        }
+        return angle;
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HexClosest.java b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HexClosest.java
new file mode 100644
index 0000000..b98e0e3
--- /dev/null
+++ b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/HexClosest.java
@@ -0,0 +1,75 @@
+package org.carlmontrobotics.commands.ShooterCommands;
+
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.util.Units;
+
+public class HexClosest {
+    private static final Pose2d[] hexVerticesBlue = {
+        new Pose2d(Units.inchesToMeters(155.491863), Units.inchesToMeters(159.249827), new Rotation2d()), 
+        new Pose2d(Units.inchesToMeters(169.870573), Units.inchesToMeters(180.359655), new Rotation2d()),
+        new Pose2d(Units.inchesToMeters(193.838106), Units.inchesToMeters(180.228677), new Rotation2d()),
+        new Pose2d(Units.inchesToMeters(205.837629), Units.inchesToMeters(159.444893), new Rotation2d()),
+        new Pose2d(Units.inchesToMeters(193.967292), Units.inchesToMeters(138.622912), new Rotation2d()),
+        new Pose2d(Units.inchesToMeters(169.661894), Units.inchesToMeters(138.558823), new Rotation2d())};
+    private static final Pose2d[] hexVerticesRed = {
+        new Pose2d(Units.inchesToMeters(443.907204), Units.inchesToMeters(159.093750), new Rotation2d()), 
+        new Pose2d(Units.inchesToMeters(455.906727), Units.inchesToMeters(179.979820), new Rotation2d()),
+        new Pose2d(Units.inchesToMeters(480.123542), Units.inchesToMeters(179.864588), new Rotation2d()),
+        new Pose2d(Units.inchesToMeters(491.993879), Units.inchesToMeters(159.042607), new Rotation2d()),
+        new Pose2d(Units.inchesToMeters(480.123542), Units.inchesToMeters(138.322912), new Rotation2d()),
+        new Pose2d(Units.inchesToMeters(456.026823), Units.inchesToMeters(138.230132), new Rotation2d())};
+    /**
+     * Returns vector (dx, dy) from robot to closest point on hex boundary.
+     * @param robot provides position of robot and direction
+     * @param isRed is a boolean determining which hub is the user going for
+     */
+    public static Translation2d closestVectorToHex(Pose2d robot, boolean isRed) {
+        Pose2d[] hexVertices;
+        if (isRed) {
+            hexVertices = hexVerticesRed;
+        }
+        else {
+            hexVertices = hexVerticesBlue;
+        }
+        Translation2d p = robot.getTranslation();
+
+        Translation2d bestPoint = null;
+        double bestDistSq = Double.POSITIVE_INFINITY;
+
+        for (int i = 0; i < 6; i++) {
+            Translation2d a = hexVertices[i].getTranslation();
+            Translation2d b = hexVertices[(i + 1) % 6].getTranslation();
+
+            // ---- closest point on segment a→b ----
+            Translation2d ab = b.minus(a);
+            Translation2d ap = p.minus(a);
+
+            double abLenSq = ab.getX()*ab.getX() + ab.getY()*ab.getY();
+            Translation2d candidate;
+
+            if (abLenSq == 0) {
+                candidate = a;
+            } else {
+                double t = (ap.getX()*ab.getX() + ap.getY()*ab.getY()) / abLenSq;
+                t = Math.max(0, Math.min(1, t));
+
+                candidate = new Translation2d(
+                        a.getX() + ab.getX() * t,
+                        a.getY() + ab.getY() * t
+                );
+            }
+            // --------------------------------------
+
+            double dx = candidate.getX() - p.getX();
+            double dy = candidate.getY() - p.getY();
+            double distSq = dx*dx + dy*dy;
+
+            if (distSq < bestDistSq) {
+                bestDistSq = distSq;
+                bestPoint = candidate;
+            }
+        }
+
+        return bestPoint.minus(p); // (x, y) components to hex
+    }
+}
diff --git a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java
index d1dba2e..200a499 100644
--- a/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java
+++ b/src/main/java/org/carlmontrobotics/commands/ShooterCommands/ShotCalculator.java
@@ -3,6 +3,7 @@
 import static org.carlmontrobotics.Constants.Shooterc.*;
 
 public class ShotCalculator {
+    private static double clearanceSafety = 0.1651;           // extra margin (m)
 
     public static class ShotResult {
         public double requiredRPM;
@@ -16,10 +17,8 @@ public static class ShotResult {
     public static ShotResult calculateShot(
             double dx, double dy, double dz,          // displacement to goal (m)
             double vxRobot, double vyRobot,           // robot velocity field-relative (m/s)
-            double wheelRadiusMeters,                 // flywheel radius
             double[] obstacleDistances,               // horizontal distance to each obstacle
-            double[] obstacleHeights,                 // required height at each obstacle
-            double clearanceSafety                    // extra margin (m)
+            double[] obstacleHeights                 // required height at each obstacle
     ) {
 
         ShotResult result = new ShotResult();
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Indexer.java b/src/main/java/org/carlmontrobotics/subsystems/Indexer.java
new file mode 100644
index 0000000..7d1b4f6
--- /dev/null
+++ b/src/main/java/org/carlmontrobotics/subsystems/Indexer.java
@@ -0,0 +1,17 @@
+// 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 org.carlmontrobotics.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Indexer extends SubsystemBase {
+  /** Creates a new Indexer. */
+  public Indexer() {}
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Shooter.java b/src/main/java/org/carlmontrobotics/subsystems/Shooter.java
index 4b0472d..52b28f6 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Shooter.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Shooter.java
@@ -55,11 +55,11 @@ private void configureMotors() {
   }
 
   public void setVoltage(double voltage) {
-    masterShooter.setVoltage(voltage);
+    pidController.setSetpoint(voltage, ControlType.kVoltage);
   }
 
   public void setVoltagePercentage(double percentage) {
-    masterShooter.set(percentage);
+    pidController.setSetpoint(percentage, ControlType.kDutyCycle);
   }
 
   public void setRPMGoal(double goal) {
@@ -67,6 +67,10 @@ public void setRPMGoal(double goal) {
     pidController.setSetpoint(goal, ControlType.kVelocity);
   }
 
+  public void stop() {
+    pidController.setSetpoint(0, ControlType.kDutyCycle);
+  }
+
   public void turnOnManualMode() {
     if (!manualMode) {
       manualMode = true;