Applied formatting to april_tags to fix workflow fail

src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc

@@ -1,6 +1,5 @@
 #include "frc846/control/hardware/SparkMXFX_interm.h"
 
-
 #include "frc846/wpilib/util.h"
 
 namespace frc846::control::hardware {
@@ -17,7 +16,6 @@ namespace frc846::control::hardware {
 #define APPLY_CONFIG_NO_RESET() \
   set_last_error(esc_->Configure(configs, NO_CONFIG_RESET, NO_PERSIST_PARAMS))
 
-
 bool SparkMXFX_interm::VerifyConnected() {
   if (esc_ == nullptr) return false;
   esc_->GetFirmwareVersion();

src/frc846/cpp/frc846/robot/calculators/AprilTagCalculator.cc

@@ -1,8 +1,9 @@
 #include "frc846/robot/calculators/AprilTagCalculator.h"
 
+#include <iostream>
+
 #include "frc846/robot/GenericRobot.h"
 #include "frc846/wpilib/time.h"
-#include <iostream>
 
 namespace frc846::robot::calculators {
 
@@ -19,9 +20,7 @@ ATCalculatorOutput AprilTagCalculator::calculate(ATCalculatorInput input) {
     units::second_t tl =
         units::second_t(constants_.april_tables.at(i)->GetNumber("tl", -1)) +
         delay;
-    if (delay > 3.5*frc846::robot::GenericRobot::kPeriod) {
-      continue; 
-    }
+    if (delay > 3.5 * frc846::robot::GenericRobot::kPeriod) { continue; }
 
     std::vector<double> tx_nums =
         constants_.april_tables.at(i)->GetNumberArray("tx", {});
@@ -44,8 +43,9 @@ ATCalculatorOutput AprilTagCalculator::calculate(ATCalculatorInput input) {
     if (distances.size() == tx.size() && tx.size() == tags.size()) {
       for (int j = 0; j < tags.size(); j++) {
         if (constants_.tag_locations.contains(tags[j])) {
-          output.pos += getPos(
-              bearingAtCapture, tx.at(j), distances.at(j), tags.at(j), i) *(48) / distances.at(j).to<double>();
+          output.pos += getPos(bearingAtCapture, tx.at(j), distances.at(j),
+                            tags.at(j), i) *
+                        (48) / distances.at(j).to<double>();
           variance +=
               1 /
               std::max(

src/frc846/cpp/frc846/robot/swerve/drivetrain.cc

@@ -23,7 +23,6 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
   RegisterPreference("steer_gains/_kD", 0.0);
   RegisterPreference("steer_gains/_kF", 0.0);
 
-
   RegisterPreference("bearing_gains/_kP", 0.5);
   RegisterPreference("bearing_gains/_kI", 0.0);
   RegisterPreference("bearing_gains/_kD", 0.0);
@@ -46,7 +45,6 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
 
   RegisterPreference("steer_lag", 0.05_s);
 
-
   RegisterPreference("pose_estimator/pose_variance", 0.01);
   RegisterPreference("pose_estimator/velocity_variance", 1.0);
   RegisterPreference("pose_estimator/accel_variance", 1.0);
@@ -56,7 +54,6 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
 
   RegisterPreference("rc_control_speed", 2.5_fps);
 
-
   odometry_.setConstants({});
   ol_calculator_.setConstants({
       .wheelbase_horizontal_dim = configs.wheelbase_horizontal_dim,
@@ -231,7 +228,7 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
       .position = {pose_estimator.position()[0], pose_estimator.position()[1]},
       .bearing = bearing,
       .velocity = pose_estimator.velocity(),
-  };  // initialize so it can be used for april tags
+  };  // Initialize estimated pose
 
   frc846::robot::calculators::ATCalculatorOutput tag_pos =
       tag_pos_calculator.calculate({estimated_pose, yaw_rate,
@@ -257,7 +254,7 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
       .position = {pose_estimator.position()[0], pose_estimator.position()[1]},
       .bearing = bearing,
       .velocity = pose_estimator.velocity(),
-  };  // update after april tags
+  };  // Update estimated pose again with vision data
 
   Graph("estimated_pose/position_x", estimated_pose.position[0]);
   Graph("estimated_pose/position_y", estimated_pose.position[1]);

src/frc846/cpp/frc846/robot/swerve/odometry/pose_estimator.cc

@@ -17,12 +17,9 @@ PoseEstimator::PoseEstimator(
   double dt =
       units::second_t(frc846::robot::GenericRobot::kPeriod).to<double>();
   filter = frc846::math::LinearKalmanFilter<6>(state_,
-      Eigen::Matrix<double, 6, 6>(
-          {{1, 0, dt, 0, dt * dt / 2, 0}, {0, 1, 0, dt, 0, dt * dt / 2},
-              {0, 0, 1, 0, dt, 0}, {0, 0, 0, 1, 0, dt}, {0, 0, 0, 0, 1, 0},
-              {0, 0, 0, 0, 0, 1}}));  // TODO: how to
-                                      // fix this to
-                                      // be prefs?
+      Eigen::Matrix<double, 6, 6>({{1, 0, dt, 0, dt * dt / 2, 0},
+          {0, 1, 0, dt, 0, dt * dt / 2}, {0, 0, 1, 0, dt, 0},
+          {0, 0, 0, 1, 0, dt}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}));
   Hv = Eigen::Matrix<double, 2, 6>({{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}});
 
   Ha = Eigen::Matrix<double, 2, 6>({{0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}});
@@ -82,4 +79,4 @@ double PoseEstimator::getVariance() {
   return (cov.coeff(0, 0) + cov.coeff(1, 1)) / 2;
 }
 
-}  // namespace frc846
+}  // namespace frc846::robot::swerve::odometry

src/frc846/include/frc846/robot/swerve/drivetrain.h

@@ -106,9 +106,9 @@ class DrivetrainSubsystem
   frc846::robot::swerve::control::SwerveOpenLoopCalculator ol_calculator_;
   frc846::robot::calculators::AprilTagCalculator tag_pos_calculator;
   frc846::robot::swerve::odometry::PoseEstimator pose_estimator{{0_ft, 0_ft},
-                                      {0_fps, 0_fps},
-                                      {units::feet_per_second_squared_t(0),
-                                       units::feet_per_second_squared_t(0)}};
+      {0_fps, 0_fps},
+      {units::feet_per_second_squared_t(0),
+          units::feet_per_second_squared_t(0)}};
 
   bool first_loop = true;
 };