Build Season Week 4: Update Simulation Code and Push Vision Setup to 'Master' by Kachow2323 · Pull Request #2 · bobabots253/Code-2026

Kachow2323 · 2026-02-04T07:31:13Z

Vision

LL3G's: Blue, Green | LL4's: Purple, Orange
Setup Vision Rigging for Global and Local Pose Estimation.
Dynamic Vision-Pose Estimate Fusing with Odometry based on Trust Factors

Fuel and Shooter Simulation

Physics Simulation of FUEL Projectiles with Collision Checks
Shooter Subsystem Simulation used to shoot and intake FUEL in simulation space

SOTF Shot Calculation

'Shoot On The Fly' Capabilities factoring in ShotTime, ChassisVelocity, and ChassisAcceleration.
Added Alliance Based Hub Angle Lock
Note: Acceleration Compensation Factor may not be needed, currently under-dampened PID.

Templates/Source: FRC 6328, 5000, etc.

…e-2026 into test/2026/vision

Kachow2323 · 2026-02-04T17:53:33Z

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

  private static final double DRIVE_AND_SHOOT_ANGLE_D = 0.4; // experimental value
-  private static final double ANGLE_MAX_VELOCITY = 8.0;
-  private static final double ANGLE_MAX_ACCELERATION = 20.0;
+  private static final double ANGLE_MAX_VELOCITY = 999;


Revision: Return to reasonable values
Units: Rad/Sec

Kachow2323 · 2026-02-04T17:53:55Z

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

-  private static final double ANGLE_MAX_VELOCITY = 8.0;
-  private static final double ANGLE_MAX_ACCELERATION = 20.0;
+  private static final double ANGLE_MAX_VELOCITY = 999;
+  private static final double ANGLE_MAX_ACCELERATION = 999;


Revision: Return to reasonable values
Units: Rad/Sec^2

Kachow2323 · 2026-02-04T17:54:37Z

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

-            0.0,
-            ANGLE_KD,
-            new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION));
+    PIDController angleController = new PIDController(5, 0.0, 0.0); // Broken atm


Revision: Tune this kP parameter

On my branch there is tuned values for the pidController (fuelJosh)

Kachow2323 · 2026-02-04T17:55:56Z

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

+  //         .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians()));
+  //   }
+
  public static Command joystickDriveAndShootHub(


Revision: Unused Command, Rm post-merge

Kachow2323 · 2026-02-04T17:58:29Z

src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java

+
+  public static enum FlywheelIOOutputMode {
+    COAST,
+    DUTY_CYCLE_BANG_BANG,


Revisiting Flywheel Implementation Concepts:

Knowns: System with X (2) DC motors with a spinning mass that has non-negligible inertia.

Voltage Compensation: Current Spikes result in downturn of Nominal Voltage:

Thus, use normalized voltage relative to the powered rail
Vnew = Vcmd (Vnominal/Vrail)
Note: Reciprocal can also work (even possibly better): Vnew = Vcmd (Vrail/Vnominal)

PID Controversy:

In typical postional control, large values of Kp can lead to overshoot and Kd is commonly used to reduce overshoots.
But does Kd in Velocity control provide sufficient damping of Kp?
PID is 1st and 2nd Order Control Loops, which gives it 3 degrees of freedom
Kp and Kd for target acquisition, kI for steady-state error removal
"With higher order systems like a one input, seven state system, there aren’t enough degrees of freedom to place the system’s poles in desired locations. This will result in poor control."
Why?
Math in the PID doesn't factor in Motors or Voltage.
Meaningfully only figures out the intergral and derivative of the original input/setpoint
But since we are using Motors, which are 2nd order, PID can work out.
Note: V = kP(angular rotation setpoint - current angular rotation)
Thus, kD is often unnecessary in Flywheel control.
It may be used to compensate for accelerating projectiles slowing down the flywheel at the cost of recovery time.
"Kd drives the acceleration to zero in the undesired case of negative acceleration as well as well as the actually desired case of positive acceleration"

Derived from 6328's Flywheel IO's:

Kachow2323 · 2026-02-04T18:30:41Z

src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java

+    double atanParam = deltaY / deltaX;
+
+    if (isRedAlliance()) {
+      angleToTargetRad = Math.atan(atanParam) - Math.PI + (Math.PI / 2); // Testing: + (Math.PI / 2)


Revision: Angular Offset to be removed on Competition Robot.

Kachow2323 · 2026-02-04T18:30:55Z

src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java

+    if (isRedAlliance()) {
+      angleToTargetRad = Math.atan(atanParam) - Math.PI + (Math.PI / 2); // Testing: + (Math.PI / 2)
+    } else {
+      angleToTargetRad = Math.atan(atanParam) - (Math.PI / 2); // Testing: + (Math.PI / 2)


Revision: Angular Offset to be removed on Competition Robot.

Kachow2323 · 2026-02-04T18:31:58Z

src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java

+  @Override
+  public void periodic() {
+    robotPose = swerveSubsystem.getPose();
+    updateTargetByAlliance();


Revision: Optimize this function to reduce jitter in Periodic block.

Kachow2323 · 2026-02-04T18:33:17Z

src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java

+        targetLocation = new Pose3d(blueHubTarget, new Rotation3d());
+      }
+    } else {
+      targetLocation = new Pose3d(blueHubTarget, new Rotation3d());


Note: Required field. Robot Init doesn't like targetLocation to initialized as blank.

Kachow2323 · 2026-02-04T18:38:44Z

src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java

 public class SwerveConstants {
  public static final double maxSpeedMetersPerSec = 5.74; // <- Check
-  public static final double odometryFrequency = 50.0; // Hz, tune
+  public static final double odometryFrequency = 250; // Hz, Test faster than periodic block timing


Note: To be tested for consistency boost. Higher sampling rates have been quoted for reducing standard deviation error in odometry-restricted autonomous pathing like PathPlanner routes without vision-estimate corrections. Does not affect accuracy (Mean Error).

Kachow2323 · 2026-02-04T18:40:02Z

src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java

  private SwerveDrivePoseEstimator poseEstimator =
      new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero);

+  boolean isFlipped =


Note: Critical patch that fixes MT2 measurements. Tested on Red DS Hub Tags (9, 10).

Additionally, this also corrects for Gyro-Flipping used in JoyStickDrive after strict trust factors were enabled on all Vision-Pose estimates (MT1 and MT2). Prior to this patch, we were unaware that our trust factors for angular measurements were being bypassed and accepted into the addVisionMeasurement queue. This caused the robot to correct for the unhandled Alliance Flip which we assumed was already handled in the SwerveSubsystem.

Kachow2323 · 2026-02-04T18:50:55Z

src/main/java/frc/robot/subsystems/vision/rewrite/FiducialObservation.java

@@ -0,0 +1,82 @@
+package frc.robot.subsystems.vision.rewrite;


Revision: RM Post-Merge

Kachow2323 · 2026-02-04T18:51:02Z

src/main/java/frc/robot/subsystems/vision/rewrite/MegatagPoseEstimate.java

@@ -0,0 +1,116 @@
+package frc.robot.subsystems.vision.rewrite;


Revision: RM Post-Merge

Kachow2323 · 2026-02-04T18:51:08Z

src/main/java/frc/robot/subsystems/vision/rewrite/TestingVision.java

@@ -0,0 +1,10 @@
+package frc.robot.subsystems.vision.rewrite;


Revision: RM Post-Merge

Kachow2323 · 2026-02-04T18:51:13Z

src/main/java/frc/robot/subsystems/vision/rewrite/TestingVisionConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.subsystems.vision.rewrite;


Revision: RM Post-Merge

Kachow2323 · 2026-02-04T18:51:18Z

src/main/java/frc/robot/subsystems/vision/rewrite/TestingVisionIO.java

@@ -0,0 +1,30 @@
+package frc.robot.subsystems.vision.rewrite;


Revision: RM Post-Merge

Kachow2323 · 2026-02-04T18:51:23Z

src/main/java/frc/robot/subsystems/vision/rewrite/TestingVisionIOLimelight.java

@@ -0,0 +1,51 @@
+package frc.robot.subsystems.vision.rewrite;


Revision: RM Post-Merge

Kachow2323 · 2026-02-04T18:57:11Z

src/main/java/frc/robot/subsystems/vision/Vision.java

+
+  @Override
+  public void periodicAfterScheduler() {
+    System.gc();


Note: Seems to solve initial crashing errors seen in Week 2 of Build Season. No issues so far.

Kachow2323 · 2026-02-04T18:58:47Z

src/main/java/frc/robot/util/fuelSimUtil/FuelSim.java

+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+
+public class FuelSim {


Note: Wholesale Import from FRC 5000

Kachow2323 · 2026-02-04T18:59:10Z

src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyCalculator.java

+   * Generates a rough estimate of the time it will take for a projectile to reach a target. This helps seed the
+   * iterative calculation of the effective target location.
+   */
+  public static double getCrappyTimeToShoot(Pose2d robotPose, Pose3d targetPose) {


Revision: Rm Post-Merge

Kachow2323 · 2026-02-04T19:00:26Z

src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyCalculator.java

+      Pose3d robotPose,
+      Pose3d targetPose,
+      ChassisSpeeds fieldRelRobotVelocity,
+      ChassisAccelerations fieldRelRobotAcceleration,


Revision: Initial testing shows that Acceleration Compensation may not be needed and is likely the source of over-compensation.

Kachow2323 · 2026-02-04T19:01:29Z

src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java

+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+
+public class ShootOnTheFlyConstants {


Note: FuelSim shows that based on input Velocity and the Angle, these parameters look promising.

Kachow2323 and others added 30 commits January 15, 2026 11:35

1/15 - Initialize Vision Testing Branch

4e95cfa

1/16 - Initialize Photonlib within Vision Subsystem (WIP)

ae13b8f

1.16.26 Field setup of all the april tags in one treemap

f957a30

1/16 - WIP 2026 Vision Testing

364ae59

Merge branch 'test/2026/vision' of https://github.com/bobabots253/Cod…

1691ec1

…e-2026 into test/2026/vision

1/18 - Update Vision Subsystems for LimelightHardwareIO

75c6a8f

1/18 - Add Hub Center into FieldConstants

82e1f74

1/18 - Add Shooting Util WIP

de4ce8e

1/18 - Fix Build Failing

b46fb90

1/20 - Removed Unused Shooter Util

d56a685

1/21 - Added getTimeToShootUsingPhysics in ShootingUtil

48272ee

1/21 - Update PhotonLib

55fd5dd

1/22 - Decent Vision Std Devs, Rm IMU Mode Testing for Future

91b3687

1/22 - Set SparkOdometryThread loop timing and LL IMU Modes

203cb4b

1/32 - Add Hammerhead FuelSim WIP

764d0e6

1/23 - Fix Misplaced FuelSim Directory

2111739

1/23 - Add fuelSim Chassis Config

75ca33c

1/23 - Quick Save Flywheel Subsystem

c9b57fe

1/24 - Quick Save Hood Subsystem

ac5f540

1/24 - Quick Save ShotCalculator Integration

fddfee1

1/24 - WIP Auto Aim, FAAAAH

6f124d1

1/24 - Fix joystickDriveAtAngle for Hub Shooting within Alliance Zone

25b86eb

1/24 - KitBot Testing Config

c41161e

1/25 - Quick Save FuelSim, Intake WIP

8e3b95e

1/25 - Add Fake Intake w/out Subsystem Creation

57df2e6

1/25 - Fix Single Jointed Arm Std Devs

3b40a84

1/25 - Trajectory exist

251a8dd

1/25 - Add Testing Button Bindings

4740dfc

1/25 - Fixed Bugged Strafing Periodic Block Calls

c6c1eea

1/28 - Update Drive Offset + Unusable AngleKP in Angle Lock Mode

768aed3

Kachow2323 added 5 commits February 2, 2026 21:09

2/2 - Soft Reset Values

e1a48fd

2/2 - Bug Fix: Alliance Flip Util for LimelightIO's for Vision

05faa23

2/3- Stable Vision Settings

f392acc

2/3 - Rm Unused Sendable Chooser and Vision Suppliers

2473359

2/3 - Build Constants, Bruh

4e5a16b

Kachow2323 self-assigned this Feb 4, 2026

Kachow2323 commented Feb 4, 2026

View reviewed changes

Merge branch 'master' into test/2026/VisionV2

5ba876b

Kachow2323 merged commit 26d91c4 into master Feb 5, 2026
2 checks passed

		@@ -0,0 +1,82 @@
		package frc.robot.subsystems.vision.rewrite;

		@@ -0,0 +1,116 @@
		package frc.robot.subsystems.vision.rewrite;

		@@ -0,0 +1,10 @@
		package frc.robot.subsystems.vision.rewrite;

		@@ -0,0 +1,15 @@
		package frc.robot.subsystems.vision.rewrite;

		@@ -0,0 +1,30 @@
		package frc.robot.subsystems.vision.rewrite;

		@@ -0,0 +1,51 @@
		package frc.robot.subsystems.vision.rewrite;

Conversation

Kachow2323 commented Feb 4, 2026

Vision

Fuel and Shooter Simulation

SOTF Shot Calculation

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Revisiting Flywheel Implementation Concepts:

Knowns: System with X (2) DC motors with a spinning mass that has non-negligible inertia.

Voltage Compensation: Current Spikes result in downturn of Nominal Voltage:

PID Controversy:

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants