Sotm acceleration prediction

[Fatblabs]

Utilizes Pigeon2 imu alongside drivebase chassis speeds for acceleration prediction

[Fatblabs]

/gemini review

[gemini-code-assist]

Code Review

This pull request introduces a new kinematic filtering system for the drivetrain, including a 3-state Kalman filter (KinematicFilterInfused) and a 2-state filter (KinematicFilter), along with associated updates to the vision subsystem and turret control logic. The review identified a critical issue in the KinematicFilter implementation regarding the state transition matrix, a high-severity concern regarding the removal of turret angular velocity feedforward in the launch calculator, and a medium-severity issue where control binding comments in the Controls class did not match the actual implementation.

I am having trouble creating individual review comments. Click here to see my feedback.

src/main/java/frc/robot/util/KinematicFilter.java (46-53)

The state transition matrix A is incorrect for a discrete-time Kalman filter. The matrix [[0, 1], [0, 0]] is for a continuous-time system (x_dot = Ax). When used in the discrete-time prediction x_k = A * x_{k-1}, it results in an incorrect state update where velocity_k = acceleration_{k-1} and acceleration_k = 0.

The correct discrete-time state transition matrix for a constant acceleration model is [[1, dt], [0, 1]]. Since dt is variable, this matrix is time-varying. WPILib's KalmanFilter class does not support a time-varying A matrix.

This bug makes the filter's state estimation incorrect. I recommend implementing the filter equations manually, similar to how it's done in KinematicFilterInfused.java, to correctly handle the time-varying A matrix.

src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java (365-372)

The turret angular velocity feedforward calculation has been removed in this refactoring. This feedforward is important for shoot-on-the-move accuracy, as it helps the turret track the target while the robot base is rotating. Without it, the turret's PID controller has to do all the work to compensate for the robot's rotation, which can result in larger tracking errors. Was the removal of this feedforward intentional? If not, it should be re-implemented. Per repository guidelines, consider passing these dynamic drivetrain values via a Supplier to ensure the calculation always uses the most up-to-date state.

References

1. When a subsystem's target or calculation depends on a continuously changing state (e.g., robot pose or velocity), use a Supplier to ensure the subsystem can dynamically update its values and avoid using stale data.

src/main/java/frc/robot/Controls.java (567-586)

The comments for the SysId bindings are out of sync with the code. For example, the comment says // Y = quasistatic forward, but the code binds this to povUp(). This is confusing for anyone trying to run the SysId characterization. Please update the comments to match the actual controls used.

    // POV Up = quasistatic forward
    connected(launcherTuningController)
        .and(launcherTuningController.povUp())
        .whileTrue(s.turretSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));

    // POV Down = quasistatic reverse
    connected(launcherTuningController)
        .and(launcherTuningController.povDown())
        .whileTrue(s.turretSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));

    // POV Right = dynamic forward
    connected(launcherTuningController)
        .and(launcherTuningController.povRight())
        .whileTrue(s.turretSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));

    // POV Left = dynamic reverse
    connected(launcherTuningController)
        .and(launcherTuningController.povLeft())
        .whileTrue(s.turretSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
  }