Run clang-format on the entire repo
This patch clang-formats the entire repo. Third-party code is
excluded.
I needed to fix up the .clang-format file so that all the header
includes are ordered properly. I could have sworn that it used to work
without the extra modification, but I guess not.
Signed-off-by: Philipp Schrader <philipp.schrader@gmail.com>
Change-Id: I64bb9f2c795401393f9dfe2fefc4f04cb36b52f6
diff --git a/frc971/analysis/in_process_plotter_demo.cc b/frc971/analysis/in_process_plotter_demo.cc
index 492ddfa..99e0c1d 100644
--- a/frc971/analysis/in_process_plotter_demo.cc
+++ b/frc971/analysis/in_process_plotter_demo.cc
@@ -1,6 +1,5 @@
-#include "frc971/analysis/in_process_plotter.h"
-
#include "aos/init.h"
+#include "frc971/analysis/in_process_plotter.h"
// To run this example, do:
// bazel run -c opt //frc971/analysis:in_process_plotter_demo
diff --git a/frc971/analysis/local_foxglove.cc b/frc971/analysis/local_foxglove.cc
index 33e5a8b..ade56a5 100644
--- a/frc971/analysis/local_foxglove.cc
+++ b/frc971/analysis/local_foxglove.cc
@@ -1,6 +1,7 @@
+#include "glog/logging.h"
+
#include "aos/init.h"
#include "aos/seasocks/seasocks_logger.h"
-#include "glog/logging.h"
#include "internal/Embedded.h"
#include "seasocks/Server.h"
diff --git a/frc971/can_logger/asc_logger.h b/frc971/can_logger/asc_logger.h
index 213aa25..19a1cb9 100644
--- a/frc971/can_logger/asc_logger.h
+++ b/frc971/can_logger/asc_logger.h
@@ -4,11 +4,12 @@
#include <iomanip>
#include <iostream>
-#include "aos/events/event_loop.h"
-#include "frc971/can_logger/can_logging_generated.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
+#include "aos/events/event_loop.h"
+#include "frc971/can_logger/can_logging_generated.h"
+
namespace frc971 {
namespace can_logger {
diff --git a/frc971/codelab/basic.h b/frc971/codelab/basic.h
index c0715a7..3aac567 100644
--- a/frc971/codelab/basic.h
+++ b/frc971/codelab/basic.h
@@ -1,12 +1,12 @@
#ifndef FRC971_CODELAB_BASIC_H_
#define FRC971_CODELAB_BASIC_H_
-#include "frc971/control_loops/control_loop.h"
#include "aos/time/time.h"
#include "frc971/codelab/basic_goal_generated.h"
#include "frc971/codelab/basic_output_generated.h"
#include "frc971/codelab/basic_position_generated.h"
#include "frc971/codelab/basic_status_generated.h"
+#include "frc971/control_loops/control_loop.h"
namespace frc971 {
namespace codelab {
diff --git a/frc971/codelab/basic_test.cc b/frc971/codelab/basic_test.cc
index 6f8e6cc..e9820bc 100644
--- a/frc971/codelab/basic_test.cc
+++ b/frc971/codelab/basic_test.cc
@@ -8,6 +8,8 @@
#include <chrono>
#include <memory>
+#include "gtest/gtest.h"
+
#include "aos/events/shm_event_loop.h"
#include "frc971/codelab/basic_goal_generated.h"
#include "frc971/codelab/basic_output_generated.h"
@@ -15,7 +17,6 @@
#include "frc971/codelab/basic_status_generated.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "gtest/gtest.h"
namespace frc971 {
namespace codelab {
@@ -123,8 +124,7 @@
auto builder = goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake(true);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
basic_simulation_.set_limit_sensor(false);
@@ -137,8 +137,7 @@
auto builder = goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake(true);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
basic_simulation_.set_limit_sensor(true);
@@ -154,8 +153,7 @@
auto builder = goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake(true);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
basic_simulation_.set_limit_sensor(false);
@@ -171,8 +169,7 @@
auto builder = goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake(false);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
basic_simulation_.set_limit_sensor(false);
@@ -188,8 +185,7 @@
auto builder = goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake(true);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
basic_simulation_.set_limit_sensor(true);
@@ -204,8 +200,7 @@
auto builder = goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake(false);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
basic_simulation_.set_limit_sensor(true);
@@ -234,8 +229,7 @@
auto builder = goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake(true);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
SetEnabled(false);
diff --git a/frc971/constants/constants_sender_example.cc b/frc971/constants/constants_sender_example.cc
index 6f9ba01..a0b23ce 100644
--- a/frc971/constants/constants_sender_example.cc
+++ b/frc971/constants/constants_sender_example.cc
@@ -1,12 +1,13 @@
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
#include "aos/configuration.h"
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
-#include "constants_sender_lib.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/constants/testdata/constants_data_generated.h"
#include "frc971/constants/testdata/constants_list_generated.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
DEFINE_string(config, "frc971/constants/testdata/aos_config.json",
"Path to the config.");
diff --git a/frc971/constants/constants_sender_lib.h b/frc971/constants/constants_sender_lib.h
index 48cacb5..9bd2c00 100644
--- a/frc971/constants/constants_sender_lib.h
+++ b/frc971/constants/constants_sender_lib.h
@@ -1,13 +1,14 @@
#ifndef FRC971_CONSTANTS_CONSTANTS_SENDER_H_
#define FRC971_CONSTANTS_CONSTANTS_SENDER_H_
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/network/team_number.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
namespace frc971::constants {
@@ -81,9 +82,7 @@
});
}
- const ConstantsData& constants() const {
- return *fetcher_.get();
- }
+ const ConstantsData &constants() const { return *fetcher_.get(); }
private:
aos::Fetcher<ConstantsData> fetcher_;
diff --git a/frc971/constants/constants_sender_test.cc b/frc971/constants/constants_sender_test.cc
index 512cb68..5e551c1 100644
--- a/frc971/constants/constants_sender_test.cc
+++ b/frc971/constants/constants_sender_test.cc
@@ -1,3 +1,6 @@
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
#include "aos/configuration.h"
#include "aos/events/event_loop.h"
#include "aos/events/simulated_event_loop.h"
@@ -7,8 +10,6 @@
#include "frc971/constants/constants_sender_lib.h"
#include "frc971/constants/testdata/constants_data_generated.h"
#include "frc971/constants/testdata/constants_list_generated.h"
-#include "glog/logging.h"
-#include "gtest/gtest.h"
namespace frc971::constants {
namespace testing {
@@ -84,10 +85,10 @@
constants_sender_event_loop_->MakeSender<testdata::ConstantsData>(
"/constants");
constants_sender_event_loop_->OnRun([&sender]() {
- auto builder = sender.MakeBuilder();
- builder.CheckOk(builder.Send(
- builder.MakeBuilder<testdata::ConstantsData>().Finish()));
- });
+ auto builder = sender.MakeBuilder();
+ builder.CheckOk(
+ builder.Send(builder.MakeBuilder<testdata::ConstantsData>().Finish()));
+ });
EXPECT_DEATH(event_loop_factory_.RunFor(std::chrono::seconds(1)),
"changes to constants");
}
diff --git a/frc971/control_loops/aiming/aiming.cc b/frc971/control_loops/aiming/aiming.cc
index cea2476..4cf9255 100644
--- a/frc971/control_loops/aiming/aiming.cc
+++ b/frc971/control_loops/aiming/aiming.cc
@@ -1,6 +1,7 @@
#include "frc971/control_loops/aiming/aiming.h"
#include "glog/logging.h"
+
#include "frc971/zeroing/wrap.h"
namespace frc971::control_loops::aiming {
@@ -104,8 +105,9 @@
// in the derivative of atan because xdot and ydot are the derivatives of
// robot_pos and we are working with the atan of (target_pos - robot_pos).
const double atan_diff =
- (squared_norm < 1e-3) ? 0.0 : (rel_x * rel_ydot - rel_y * rel_xdot) /
- squared_norm;
+ (squared_norm < 1e-3)
+ ? 0.0
+ : (rel_x * rel_ydot - rel_y * rel_xdot) / squared_norm;
// heading = atan2(relative_y, relative_x) - robot_theta
// dheading / dt =
// (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
diff --git a/frc971/control_loops/aiming/aiming.h b/frc971/control_loops/aiming/aiming.h
index 47fd06a..c743fe8 100644
--- a/frc971/control_loops/aiming/aiming.h
+++ b/frc971/control_loops/aiming/aiming.h
@@ -58,5 +58,5 @@
};
TurretGoal AimerGoal(const ShotConfig &config, const RobotState &state);
-}
+} // namespace frc971::control_loops::aiming
#endif // FRC971_CONTROL_LOOPS_AIMING_AIMING_H_
diff --git a/frc971/control_loops/aiming/aiming_test.cc b/frc971/control_loops/aiming/aiming_test.cc
index c1f7367..4ec7eb9 100644
--- a/frc971/control_loops/aiming/aiming_test.cc
+++ b/frc971/control_loops/aiming/aiming_test.cc
@@ -1,8 +1,9 @@
#include "frc971/control_loops/aiming/aiming.h"
-#include "frc971/control_loops/pose.h"
#include "gtest/gtest.h"
+
#include "frc971/constants.h"
+#include "frc971/control_loops/pose.h"
namespace frc971::control_loops::aiming::testing {
diff --git a/frc971/control_loops/c2d.h b/frc971/control_loops/c2d.h
index 50315e6..a11fed5 100644
--- a/frc971/control_loops/c2d.h
+++ b/frc971/control_loops/c2d.h
@@ -4,6 +4,7 @@
#include <chrono>
#include <Eigen/Dense>
+
#include "aos/time/time.h"
// We need to include MatrixFunctions for the matrix exponential.
#include "unsupported/Eigen/MatrixFunctions"
diff --git a/frc971/control_loops/c2d_test.cc b/frc971/control_loops/c2d_test.cc
index a0c4deb..14b4952 100644
--- a/frc971/control_loops/c2d_test.cc
+++ b/frc971/control_loops/c2d_test.cc
@@ -2,9 +2,10 @@
#include <functional>
-#include "frc971/control_loops/runge_kutta.h"
#include "gtest/gtest.h"
+#include "frc971/control_loops/runge_kutta.h"
+
namespace frc971 {
namespace controls {
namespace testing {
@@ -65,8 +66,9 @@
},
Eigen::Matrix<double, 2, 2>::Zero(), 0, 1.0);
EXPECT_LT((Q_d_integrated - Q_d).norm(), 1e-10)
- << "Expected these to be nearly equal:\nQ_d:\n" << Q_d
- << "\nQ_d_integrated:\n" << Q_d_integrated;
+ << "Expected these to be nearly equal:\nQ_d:\n"
+ << Q_d << "\nQ_d_integrated:\n"
+ << Q_d_integrated;
}
// Tests that the "fast" discretization produces nearly identical results.
diff --git a/frc971/control_loops/capped_test_plant.h b/frc971/control_loops/capped_test_plant.h
index f4286cd..8b4cefd 100644
--- a/frc971/control_loops/capped_test_plant.h
+++ b/frc971/control_loops/capped_test_plant.h
@@ -1,9 +1,10 @@
#ifndef FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
#define FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
-#include "frc971/control_loops/state_feedback_loop.h"
#include "gtest/gtest.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
namespace frc971 {
namespace control_loops {
@@ -27,6 +28,6 @@
double voltage_offset_ = 0.0;
};
-} // namespace frc971
} // namespace control_loops
+} // namespace frc971
#endif // FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 6e1dcf4..ccad84a 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -1,6 +1,7 @@
#include "frc971/control_loops/coerce_goal.h"
#include "Eigen/Dense"
+
#include "frc971/control_loops/polytope.h"
namespace frc971 {
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 1847682..6867540 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -2,6 +2,7 @@
#define FRC971_CONTROL_LOOPS_COERCE_GOAL_H_
#include "Eigen/Dense"
+
#include "frc971/control_loops/polytope.h"
namespace frc971 {
diff --git a/frc971/control_loops/coerce_goal_test.cc b/frc971/control_loops/coerce_goal_test.cc
index acdee9f..95520db 100644
--- a/frc971/control_loops/coerce_goal_test.cc
+++ b/frc971/control_loops/coerce_goal_test.cc
@@ -2,9 +2,10 @@
#include <unistd.h>
-#include "frc971/control_loops/polytope.h"
#include "gtest/gtest.h"
+#include "frc971/control_loops/polytope.h"
+
namespace frc971 {
namespace control_loops {
diff --git a/frc971/control_loops/control_loop.cc b/frc971/control_loops/control_loop.cc
index 99c005a..3b475a9 100644
--- a/frc971/control_loops/control_loop.cc
+++ b/frc971/control_loops/control_loop.cc
@@ -1,7 +1,5 @@
#include "frc971/control_loops/control_loop.h"
namespace frc971 {
-namespace controls {
-
-} // namespace controls
+namespace controls {} // namespace controls
} // namespace frc971
diff --git a/frc971/control_loops/control_loop_test.cc b/frc971/control_loops/control_loop_test.cc
index e28f595..6704bf5 100644
--- a/frc971/control_loops/control_loop_test.cc
+++ b/frc971/control_loops/control_loop_test.cc
@@ -3,8 +3,5 @@
#include <chrono>
namespace frc971 {
-namespace testing {
-
-
-} // namespace testing
+namespace testing {} // namespace testing
} // namespace frc971
diff --git a/frc971/control_loops/control_loop_test.h b/frc971/control_loops/control_loop_test.h
index 90c93c7..c667568 100644
--- a/frc971/control_loops/control_loop_test.h
+++ b/frc971/control_loops/control_loop_test.h
@@ -4,6 +4,9 @@
#include <chrono>
#include <string_view>
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
#include "aos/events/simulated_event_loop.h"
#include "aos/flatbuffers.h"
#include "aos/json_to_flatbuffer.h"
@@ -12,8 +15,6 @@
#include "aos/time/time.h"
#include "frc971/input/joystick_state_generated.h"
#include "frc971/input/robot_state_generated.h"
-#include "glog/logging.h"
-#include "gtest/gtest.h"
namespace frc971 {
namespace testing {
diff --git a/frc971/control_loops/double_jointed_arm/dynamics.h b/frc971/control_loops/double_jointed_arm/dynamics.h
index d4de1ed..c6b078b 100644
--- a/frc971/control_loops/double_jointed_arm/dynamics.h
+++ b/frc971/control_loops/double_jointed_arm/dynamics.h
@@ -2,9 +2,10 @@
#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DYNAMICS_H_
#include "Eigen/Dense"
-#include "frc971/control_loops/runge_kutta.h"
#include "gflags/gflags.h"
+#include "frc971/control_loops/runge_kutta.h"
+
DECLARE_bool(gravity);
namespace frc971 {
diff --git a/frc971/control_loops/double_jointed_arm/dynamics_test.cc b/frc971/control_loops/double_jointed_arm/dynamics_test.cc
index 6e12f75..5234af5 100644
--- a/frc971/control_loops/double_jointed_arm/dynamics_test.cc
+++ b/frc971/control_loops/double_jointed_arm/dynamics_test.cc
@@ -1,8 +1,9 @@
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
-#include "frc971/control_loops/double_jointed_arm/test_constants.h"
#include "gtest/gtest.h"
+#include "frc971/control_loops/double_jointed_arm/test_constants.h"
+
namespace frc971 {
namespace control_loops {
namespace arm {
diff --git a/frc971/control_loops/double_jointed_arm/ekf.cc b/frc971/control_loops/double_jointed_arm/ekf.cc
index a70e4a6..f8ec6d6 100644
--- a/frc971/control_loops/double_jointed_arm/ekf.cc
+++ b/frc971/control_loops/double_jointed_arm/ekf.cc
@@ -3,6 +3,7 @@
#include <iostream>
#include "Eigen/Dense"
+
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/jacobian.h"
diff --git a/frc971/control_loops/double_jointed_arm/ekf.h b/frc971/control_loops/double_jointed_arm/ekf.h
index d969e82..45e1641 100644
--- a/frc971/control_loops/double_jointed_arm/ekf.h
+++ b/frc971/control_loops/double_jointed_arm/ekf.h
@@ -2,6 +2,7 @@
#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_EKF_H_
#include "Eigen/Dense"
+
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
namespace frc971 {
diff --git a/frc971/control_loops/double_jointed_arm/test_constants.h b/frc971/control_loops/double_jointed_arm/test_constants.h
index 4885d14..4ca2293 100644
--- a/frc971/control_loops/double_jointed_arm/test_constants.h
+++ b/frc971/control_loops/double_jointed_arm/test_constants.h
@@ -44,10 +44,9 @@
.num_distal_motors = 2.0,
};
-} // namespace testing
-} // namespace arm
-} // namespace control_loops
-} // namespace frc971
+} // namespace testing
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
-#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TEST_CONSTANTS_H_
-
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TEST_CONSTANTS_H_
diff --git a/frc971/control_loops/double_jointed_arm/trajectory.cc b/frc971/control_loops/double_jointed_arm/trajectory.cc
index a67216b..05a2d17 100644
--- a/frc971/control_loops/double_jointed_arm/trajectory.cc
+++ b/frc971/control_loops/double_jointed_arm/trajectory.cc
@@ -1,11 +1,12 @@
#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "Eigen/Dense"
+#include "gflags/gflags.h"
+
#include "aos/logging/logging.h"
#include "frc971/control_loops/dlqr.h"
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/jacobian.h"
-#include "gflags/gflags.h"
DEFINE_double(lqr_proximal_pos, 0.15, "Position LQR gain");
DEFINE_double(lqr_proximal_vel, 4.0, "Velocity LQR gain");
@@ -394,8 +395,7 @@
const ::Eigen::Matrix<double, 4, 4> final_A =
::frc971::control_loops::NumericalJacobianX<4, 2>(
[this](const auto &x_blocked, const auto &U, double dt) {
- return this->dynamics_->UnboundedDiscreteDynamics(
- x_blocked, U, dt);
+ return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
},
x_blocked, U, 0.00505);
@@ -505,9 +505,9 @@
const ::Eigen::Matrix<double, 6, 1> R =
Trajectory::R(theta_, ::Eigen::Matrix<double, 2, 1>::Zero());
- U_ff_ = dynamics_->FF_U(
- X.block<4, 1>(0, 0), ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero());
+ U_ff_ = dynamics_->FF_U(X.block<4, 1>(0, 0),
+ ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero());
const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
U_ = U_unsaturated_ = U_ff_ + K * (R - X);
diff --git a/frc971/control_loops/double_jointed_arm/trajectory.h b/frc971/control_loops/double_jointed_arm/trajectory.h
index a194a03..ca570d2 100644
--- a/frc971/control_loops/double_jointed_arm/trajectory.h
+++ b/frc971/control_loops/double_jointed_arm/trajectory.h
@@ -7,6 +7,7 @@
#include <vector>
#include "Eigen/Dense"
+
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
namespace frc971 {
@@ -279,7 +280,6 @@
const Path &path() const { return *path_; }
-
private:
friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
diff --git a/frc971/control_loops/double_jointed_arm/trajectory_test.cc b/frc971/control_loops/double_jointed_arm/trajectory_test.cc
index e700282..99575e9 100644
--- a/frc971/control_loops/double_jointed_arm/trajectory_test.cc
+++ b/frc971/control_loops/double_jointed_arm/trajectory_test.cc
@@ -1,10 +1,11 @@
#include "frc971/control_loops/double_jointed_arm/trajectory.h"
-#include "frc971/control_loops/double_jointed_arm/test_constants.h"
+
+#include "gtest/gtest.h"
#include "frc971/control_loops/double_jointed_arm/demo_path.h"
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/double_jointed_arm/ekf.h"
-#include "gtest/gtest.h"
+#include "frc971/control_loops/double_jointed_arm/test_constants.h"
namespace frc971 {
namespace control_loops {
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index c200b55..04a1add 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -1,8 +1,9 @@
#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "glog/logging.h"
+
#include "aos/logging/logging.h"
#include "frc971/control_loops/drivetrain/spline.h"
-#include "glog/logging.h"
namespace frc971 {
namespace control_loops {
@@ -56,8 +57,8 @@
}
}
- const double dalpha =
- static_cast<double>(splines().size()) / static_cast<double>(num_alpha - 1);
+ const double dalpha = static_cast<double>(splines().size()) /
+ static_cast<double>(num_alpha - 1);
double last_alpha = 0.0;
for (size_t i = 1; i < num_alpha; ++i) {
const double alpha = dalpha * i;
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index f445fa4..63156fe 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -5,6 +5,7 @@
#include "Eigen/Dense"
#include "absl/types/span.h"
+
#include "aos/containers/sized_array.h"
#include "frc971/control_loops/drivetrain/spline.h"
#include "frc971/control_loops/drivetrain/trajectory_generated.h"
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index 0dd58ba..60f83b9 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -2,10 +2,11 @@
#include <vector>
-#include "aos/testing/test_shm.h"
-#include "aos/flatbuffers.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
+
+#include "aos/flatbuffers.h"
+#include "aos/testing/test_shm.h"
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 15d84ea..3b16273 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -7,6 +7,7 @@
#include <memory>
#include "Eigen/Dense"
+
#include "aos/logging/logging.h"
#include "frc971/control_loops/drivetrain/down_estimator.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -219,7 +220,8 @@
case GyroType::FLIPPED_SPARTAN_GYRO:
if (!yaw_gyro_zero_.has_value()) {
yaw_gyro_zeroer_.AddData(last_gyro_rate_);
- if (yaw_gyro_zeroer_.full() && yaw_gyro_zeroer_.GetRange() < kMaxYawGyroZeroingRange) {
+ if (yaw_gyro_zeroer_.full() &&
+ yaw_gyro_zeroer_.GetRange() < kMaxYawGyroZeroingRange) {
yaw_gyro_zero_ = yaw_gyro_zeroer_.GetAverage()(0);
VLOG(1) << "Zeroed to " << *yaw_gyro_zero_ << " Range "
<< yaw_gyro_zeroer_.GetRange();
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index f448760..c92b7d7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -2,6 +2,7 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
#include "Eigen/Dense"
+
#include "aos/util/log_interval.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index edda034..3e17822 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -3,6 +3,10 @@
#include <chrono>
#include <memory>
+#include "gflags/gflags.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
#include "aos/events/event_loop.h"
#include "aos/events/logging/log_writer.h"
#include "aos/time/time.h"
@@ -19,9 +23,6 @@
#include "frc971/control_loops/drivetrain/trajectory_generator.h"
#include "frc971/control_loops/polytope.h"
#include "frc971/queues/gyro_generated.h"
-#include "gflags/gflags.h"
-#include "gmock/gmock.h"
-#include "gtest/gtest.h"
DEFINE_string(output_file, "",
"If set, logs all channels to the provided logfile.");
@@ -183,8 +184,7 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(2));
VerifyNearGoal();
@@ -200,8 +200,7 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
// Sanity check that the drivetrain is indeed commanding voltage while the IMU
@@ -237,8 +236,7 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
drivetrain_plant_.set_left_voltage_offset(1.0);
drivetrain_plant_.set_right_voltage_offset(1.0);
@@ -254,8 +252,7 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
for (int i = 0; i < 500; ++i) {
if (i > 20 && i < 200) {
@@ -285,8 +282,7 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(4.0);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
for (int i = 0; i < 500; ++i) {
@@ -310,8 +306,7 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(3.9);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
for (int i = 0; i < 500; ++i) {
RunFor(dt());
@@ -372,8 +367,7 @@
goal_builder.add_right_goal(4.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
const auto start_time = monotonic_now();
@@ -415,8 +409,7 @@
goal_builder.add_right_goal(1.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
const auto start_time = monotonic_now();
@@ -458,8 +451,7 @@
goal_builder.add_right_goal(4.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
const auto start_time = monotonic_now();
@@ -482,8 +474,7 @@
goal_builder.add_throttle(1.0);
goal_builder.add_highgear(true);
goal_builder.add_quickturn(false);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(1));
@@ -496,8 +487,7 @@
goal_builder.add_throttle(-0.3);
goal_builder.add_highgear(true);
goal_builder.add_quickturn(false);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(1));
@@ -510,8 +500,7 @@
goal_builder.add_throttle(0.0);
goal_builder.add_highgear(true);
goal_builder.add_quickturn(false);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(10));
@@ -538,8 +527,7 @@
goal_builder.add_right_goal(4.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
const auto end_time = monotonic_now() + chrono::seconds(4);
@@ -590,8 +578,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -636,8 +623,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
// Check that we are right on the spline at the start (otherwise the feedback
@@ -695,8 +681,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -736,8 +721,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -749,8 +733,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
for (int i = 0; i < 100; ++i) {
RunFor(dt());
@@ -800,8 +783,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -813,8 +795,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -824,8 +805,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -889,8 +869,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(5000));
@@ -937,8 +916,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(5000));
@@ -988,8 +966,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(5000));
@@ -1049,8 +1026,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(4000));
@@ -1092,8 +1068,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1134,8 +1109,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -1175,8 +1149,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -1194,8 +1167,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -1230,8 +1202,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1305,8 +1276,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1374,8 +1344,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1385,8 +1354,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(4000));
@@ -1465,8 +1433,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1488,8 +1455,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(kRunSpline);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
for (size_t spline_index = 0;
spline_index < DrivetrainLoop::kNumSplineFetchers + kExtraSplines;
@@ -1560,8 +1526,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
goal_builder.add_throttle(0.5);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(5));
@@ -1598,8 +1563,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
goal_builder.add_throttle(0.5);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(5));
@@ -1645,8 +1609,7 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(4.0);
- ASSERT_EQ(builder.Send(goal_builder.Finish()),
- aos::RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(2));
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index a973552..ab7e280 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -2,12 +2,12 @@
#include <chrono>
+#include "gflags/gflags.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
#include "frc971/control_loops/drivetrain/trajectory.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
@@ -182,8 +182,7 @@
status_builder.add_x(state_.x());
status_builder.add_y(state_.y());
status_builder.add_theta(state_(2));
- CHECK_EQ(builder.Send(status_builder.Finish()),
- aos::RawSender::Error::kOk);
+ CHECK_EQ(builder.Send(status_builder.Finish()), aos::RawSender::Error::kOk);
}
void DrivetrainSimulation::SendPositionMessage() {
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index 356eadf..4368b00 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -12,8 +12,8 @@
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/wpilib/imu_batch_generated.h"
#include "frc971/queues/gyro_generated.h"
+#include "frc971/wpilib/imu_batch_generated.h"
namespace frc971 {
namespace control_loops {
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.h b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
index f72b4be..95a2000 100644
--- a/frc971/control_loops/drivetrain/drivetrain_uc.q.h
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
@@ -1,6 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_Q_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_Q_H_
#include <array>
+
#include "aos/macros.h"
namespace frc971 {
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index ce4e1b9..743e494 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -4,6 +4,7 @@
#include <chrono>
#include "Eigen/Dense"
+
#include "aos/commonmath.h"
#include "aos/containers/priority_queue.h"
#include "aos/util/math.h"
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 5efb297..01028c0 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -2,11 +2,12 @@
#include <random>
+#include "gtest/gtest.h"
+
#include "aos/testing/random_seed.h"
#include "aos/testing/test_shm.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/drivetrain/trajectory.h"
-#include "gtest/gtest.h"
namespace frc971 {
namespace control_loops {
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index 449c70d..71347e2 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -2,6 +2,7 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
+
#include "frc971/control_loops/quaternion_utils.h"
namespace frc971 {
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index b4b0b76..4e3227e 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -3,6 +3,7 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
+#include "glog/logging.h"
#include "aos/events/event_loop.h"
#include "aos/time/time.h"
@@ -10,7 +11,6 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/control_loops/runge_kutta.h"
-#include "glog/logging.h"
namespace frc971 {
namespace control_loops {
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index af2f3b2..376c131 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -1,14 +1,15 @@
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
-#include <Eigen/Geometry>
#include <random>
-#include "frc971/control_loops/quaternion_utils.h"
-#include "aos/testing/random_seed.h"
-#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/runge_kutta.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
+#include <Eigen/Geometry>
+
+#include "aos/testing/random_seed.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/control_loops/runge_kutta.h"
namespace frc971 {
namespace control_loops {
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index c5b03f0..bb2f7ae 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -1,6 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
#include "Eigen/Dense"
+
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 5fe6ec3..d2ebb4e 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -2,13 +2,14 @@
#include <chrono>
-#include "aos/testing/test_shm.h"
-#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/drivetrain/trajectory.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
DECLARE_bool(plot);
namespace chrono = ::std::chrono;
diff --git a/frc971/control_loops/drivetrain/localization/puppet_localizer.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
index 77b9fbe..8a60ea5 100644
--- a/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
@@ -44,9 +44,9 @@
}
void PuppetLocalizer::Update(const Eigen::Matrix<double, 2, 1> &U,
- aos::monotonic_clock::time_point now,
- double left_encoder, double right_encoder,
- double gyro_rate, const Eigen::Vector3d &accel) {
+ aos::monotonic_clock::time_point now,
+ double left_encoder, double right_encoder,
+ double gyro_rate, const Eigen::Vector3d &accel) {
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate,
U.cast<float>(), accel.cast<float>(), now);
if (localizer_output_fetcher_.Fetch()) {
diff --git a/frc971/control_loops/drivetrain/localization/puppet_localizer.h b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
index 24e2f88..9b348a7 100644
--- a/frc971/control_loops/drivetrain/localization/puppet_localizer.h
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
@@ -6,8 +6,8 @@
#include "aos/events/event_loop.h"
#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
-#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
namespace frc971 {
namespace control_loops {
diff --git a/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
index e45f880..d5f9a31 100644
--- a/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer_test.cc
@@ -2,16 +2,17 @@
#include <queue>
+#include "gtest/gtest.h"
+
#include "aos/events/logging/log_writer.h"
#include "aos/network/message_bridge_server_generated.h"
#include "aos/network/team_number.h"
#include "aos/network/testing_time_converter.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "frc971/control_loops/team_number_test_environment.h"
-#include "gtest/gtest.h"
-#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/team_number_test_environment.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
DEFINE_string(output_folder, "",
@@ -39,7 +40,6 @@
using frc971::control_loops::drivetrain::DrivetrainLoop;
using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
-
// TODO(james): Make it so this actually tests the full system of the localizer.
class LocalizedDrivetrainTest : public frc971::testing::ControlLoopTest {
protected:
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index d9e239b..10ee445 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -35,7 +35,6 @@
return (joystick_state_fetcher_.get() != nullptr)
? joystick_state_fetcher_->alliance()
: aos::Alliance::kInvalid;
-
}
std::optional<aos::monotonic_clock::duration> LocalizationUtils::ClockOffset(
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index aea030e..ecdf340 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -2,9 +2,9 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
#include "aos/commonmath.h"
-#include "frc971/control_loops/polytope.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/gear.h"
+#include "frc971/control_loops/polytope.h"
#ifdef __linux__
#include "aos/logging/logging.h"
#include "frc971/control_loops/control_loops_generated.h"
diff --git a/frc971/control_loops/drivetrain/spline_test.cc b/frc971/control_loops/drivetrain/spline_test.cc
index 015ec6a..6498fb6 100644
--- a/frc971/control_loops/drivetrain/spline_test.cc
+++ b/frc971/control_loops/drivetrain/spline_test.cc
@@ -4,6 +4,7 @@
#include "gflags/gflags.h"
#include "gtest/gtest.h"
+
#include "frc971/analysis/in_process_plotter.h"
DEFINE_bool(plot, false, "If true, plot");
@@ -15,9 +16,8 @@
std::string TestName() {
const ::testing::TestInfo *info =
- ::testing::UnitTest::GetInstance()->current_test_info();
- return std::string(info->test_case_name()) + "." +
- std::string(info->name());
+ ::testing::UnitTest::GetInstance()->current_test_info();
+ return std::string(info->test_case_name()) + "." + std::string(info->name());
}
// Test fixture with a spline from 0, 0 to 1, 1
@@ -34,6 +34,7 @@
plotter_->Spin();
}
}
+
protected:
SplineTest()
: control_points_((::Eigen::Matrix<double, 2, 4>() << 0.0, 0.5, 0.5, 1.0,
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index ddcf63d..977529f 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,6 +1,7 @@
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "Eigen/Dense"
+
#include "aos/json_to_flatbuffer.h"
#include "aos/realtime.h"
#include "aos/util/math.h"
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 71e5ffe..c528733 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -5,6 +5,7 @@
#include <thread>
#include "Eigen/Dense"
+
#include "aos/condition.h"
#include "aos/mutex/mutex.h"
#include "frc971/control_loops/control_loops_generated.h"
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 594175c..e1c4dac 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -1,12 +1,12 @@
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "aos/commonmath.h"
-#include "frc971/control_loops/polytope.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/polytope.h"
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index be42abf..77f1e25 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -2,10 +2,9 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SSDRIVETRAIN_H_
#include "aos/commonmath.h"
-#include "frc971/control_loops/control_loop.h"
-#include "frc971/control_loops/polytope.h"
#include "aos/util/trapezoid_profile.h"
#include "frc971/control_loops/coerce_goal.h"
+#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
@@ -13,6 +12,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_states.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/polytope.h"
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 33a8577..76a6784 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -2,8 +2,9 @@
#include <chrono>
-#include "aos/util/math.h"
#include "Eigen/Dense"
+
+#include "aos/util/math.h"
#include "frc971/control_loops/c2d.h"
#include "frc971/control_loops/dlqr.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index ab1c55f..e30ca98 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -4,6 +4,7 @@
#include <chrono>
#include "Eigen/Dense"
+
#include "aos/flatbuffers.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.h b/frc971/control_loops/drivetrain/trajectory_generator.h
index 0bf5e92..234d386 100644
--- a/frc971/control_loops/drivetrain/trajectory_generator.h
+++ b/frc971/control_loops/drivetrain/trajectory_generator.h
@@ -15,6 +15,7 @@
TrajectoryGenerator(aos::EventLoop *event_loop,
const DrivetrainConfig<double> &config);
void HandleSplineGoal(const SplineGoal &goal);
+
private:
aos::EventLoop *const event_loop_;
const DrivetrainConfig<double> dt_config_;
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 6fe1fbf..207df18 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -1,13 +1,13 @@
-#include "frc971/control_loops/drivetrain/trajectory.h"
-
#include <chrono>
+#include "gflags/gflags.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+
#include "aos/logging/implementations.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
#include "frc971/control_loops/dlqr.h"
-#include "gflags/gflags.h"
-#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
// Notes:
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index 806e020..82ae312 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -3,10 +3,11 @@
#include <chrono>
#include <vector>
-#include "aos/testing/test_shm.h"
-#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
+
+#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
@@ -446,8 +447,9 @@
absolute_state = RungeKuttaU(
[this](const Eigen::Matrix<double, 5, 1> &X,
const Eigen::Matrix<double, 2, 1> &U) {
- return ContinuousDynamics(finished_trajectory_->velocity_drivetrain().plant(),
- dt_config_.Tlr_to_la(), X, U);
+ return ContinuousDynamics(
+ finished_trajectory_->velocity_drivetrain().plant(),
+ dt_config_.Tlr_to_la(), X, U);
},
absolute_state, U, aos::time::DurationInSeconds(dt_config_.dt));
const Eigen::Matrix<double, 5, 1> goal_absolute_state =
diff --git a/frc971/control_loops/fixed_quadrature.h b/frc971/control_loops/fixed_quadrature.h
index 22d4984..53bb0dc 100644
--- a/frc971/control_loops/fixed_quadrature.h
+++ b/frc971/control_loops/fixed_quadrature.h
@@ -1,28 +1,29 @@
#ifndef FRC971_CONTROL_LOOPS_FIXED_QUADRATURE_H_
#define FRC971_CONTROL_LOOPS_FIXED_QUADRATURE_H_
-#include <Eigen/Dense>
#include <array>
+#include <Eigen/Dense>
+
namespace frc971 {
namespace control_loops {
-// Implements Gaussian Quadrature integration (5th order). fn is the function to
-// integrate. It must take 1 argument of type T. The integration is between a
-// and b.
+// Implements Gaussian Quadrature integration (5th order). fn is the function
+// to integrate. It must take 1 argument of type T. The integration is between
+// a and b.
template <typename T, typename F>
double GaussianQuadrature5(const F &fn, T a, T b) {
// Pulled from Python.
// numpy.set_printoptions(precision=20)
// scipy.special.p_roots(5)
- const ::std::array<double, 5> x{{
- -9.06179845938663630633e-01, -5.38469310105682885670e-01,
- 3.24607628916367383789e-17, 5.38469310105683218737e-01,
- 9.06179845938663408589e-01}};
+ const ::std::array<double, 5> x{
+ {-9.06179845938663630633e-01, -5.38469310105682885670e-01,
+ 3.24607628916367383789e-17, 5.38469310105683218737e-01,
+ 9.06179845938663408589e-01}};
- const ::std::array<double, 5> w{{
- 0.23692688505618844652, 0.4786286704993669705, 0.56888888888888811124,
- 0.47862867049936674846, 0.23692688505618875183}};
+ const ::std::array<double, 5> w{
+ {0.23692688505618844652, 0.4786286704993669705, 0.56888888888888811124,
+ 0.47862867049936674846, 0.23692688505618875183}};
double answer = 0.0;
for (int i = 0; i < 5; ++i) {
@@ -38,14 +39,14 @@
// Pulled from Python.
// numpy.set_printoptions(precision=20)
// scipy.special.p_roots(5)
- const ::std::array<double, 5> x{{
- -9.06179845938663630633e-01, -5.38469310105682885670e-01,
- 3.24607628916367383789e-17, 5.38469310105683218737e-01,
- 9.06179845938663408589e-01}};
+ const ::std::array<double, 5> x{
+ {-9.06179845938663630633e-01, -5.38469310105682885670e-01,
+ 3.24607628916367383789e-17, 5.38469310105683218737e-01,
+ 9.06179845938663408589e-01}};
- const ::std::array<double, 5> w{{
- 0.23692688505618844652, 0.4786286704993669705, 0.56888888888888811124,
- 0.47862867049936674846, 0.23692688505618875183}};
+ const ::std::array<double, 5> w{
+ {0.23692688505618844652, 0.4786286704993669705, 0.56888888888888811124,
+ 0.47862867049936674846, 0.23692688505618875183}};
Eigen::Matrix<double, N, 1> answer;
answer.setZero();
diff --git a/frc971/control_loops/gaussian_noise.cc b/frc971/control_loops/gaussian_noise.cc
index c1c8701..ddfa0f3 100644
--- a/frc971/control_loops/gaussian_noise.cc
+++ b/frc971/control_loops/gaussian_noise.cc
@@ -4,9 +4,7 @@
namespace control_loops {
GaussianNoise::GaussianNoise(unsigned int seed, double stddev)
- : stddev_(stddev),
- generator_(seed),
- distribution_(0.0, 1.0) {
+ : stddev_(stddev), generator_(seed), distribution_(0.0, 1.0) {
// Everything is initialized now.
}
diff --git a/frc971/control_loops/gaussian_noise.h b/frc971/control_loops/gaussian_noise.h
index 8adf840..dce60fb 100644
--- a/frc971/control_loops/gaussian_noise.h
+++ b/frc971/control_loops/gaussian_noise.h
@@ -19,9 +19,7 @@
double AddNoiseToSample(double sample);
// Sets the standard deviation of the gaussian noise.
- inline void set_standard_deviation(double stddev) {
- stddev_ = stddev;
- }
+ inline void set_standard_deviation(double stddev) { stddev_ = stddev; }
private:
double stddev_;
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 46e6a05..9d463d8 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -8,6 +8,7 @@
#include <vector>
#include "Eigen/Dense"
+
#include "aos/logging/logging.h"
#include "aos/macros.h"
#include "frc971/control_loops/c2d.h"
@@ -238,10 +239,12 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
&R_continuous,
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
- &P_steady_state, size_t delayed_u)
+ &P_steady_state,
+ size_t delayed_u)
: Q_continuous(Q_continuous),
R_continuous(R_continuous),
- P_steady_state(P_steady_state), delayed_u(delayed_u) {
+ P_steady_state(P_steady_state),
+ delayed_u(delayed_u) {
CHECK(!delayed_u) << ": Delayed hybrid filters aren't supported yet.";
}
};
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 147a84f..76e2838 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -1,8 +1,9 @@
#include "frc971/control_loops/hybrid_state_feedback_loop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "gtest/gtest.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
namespace frc971 {
namespace control_loops {
namespace testing {
diff --git a/frc971/control_loops/jacobian.h b/frc971/control_loops/jacobian.h
index 8b0d4a5..0940c6c 100644
--- a/frc971/control_loops/jacobian.h
+++ b/frc971/control_loops/jacobian.h
@@ -28,7 +28,7 @@
template <int num_states, int num_u, typename F, typename... Args>
::Eigen::Matrix<double, num_states, num_states> NumericalJacobianX(
const F &fn, ::Eigen::Matrix<double, num_states, 1> X,
- ::Eigen::Matrix<double, num_u, 1> U, Args &&... args) {
+ ::Eigen::Matrix<double, num_u, 1> U, Args &&...args) {
return NumericalJacobian<num_states, num_states>(
[&](::Eigen::Matrix<double, num_states, 1> X) {
return fn(X, U, args...);
@@ -40,7 +40,7 @@
template <int num_states, int num_u, typename F, typename... Args>
::Eigen::Matrix<double, num_states, num_u> NumericalJacobianU(
const F &fn, ::Eigen::Matrix<double, num_states, 1> X,
- ::Eigen::Matrix<double, num_u, 1> U, Args &&... args) {
+ ::Eigen::Matrix<double, num_u, 1> U, Args &&...args) {
return NumericalJacobian<num_states, num_u>(
[&](::Eigen::Matrix<double, num_u, 1> U) { return fn(X, U, args...); },
U);
diff --git a/frc971/control_loops/jacobian_test.cc b/frc971/control_loops/jacobian_test.cc
index 6046c37..57c9d7f 100644
--- a/frc971/control_loops/jacobian_test.cc
+++ b/frc971/control_loops/jacobian_test.cc
@@ -7,8 +7,8 @@
namespace testing {
::Eigen::Matrix<double, 4, 4> A = (::Eigen::Matrix<double, 4, 4>() << 1, 2, 4,
- 1, 5, 2, 3, 4, 5, 1, 3, 2, 1, 1, 3,
- 7).finished();
+ 1, 5, 2, 3, 4, 5, 1, 3, 2, 1, 1, 3, 7)
+ .finished();
::Eigen::Matrix<double, 4, 2> B =
(::Eigen::Matrix<double, 4, 2>() << 1, 1, 2, 1, 3, 2, 3, 7).finished();
diff --git a/frc971/control_loops/polytope.h b/frc971/control_loops/polytope.h
index 1d707d4..794219b 100644
--- a/frc971/control_loops/polytope.h
+++ b/frc971/control_loops/polytope.h
@@ -4,8 +4,12 @@
#include "Eigen/Dense"
#ifdef __linux__
+// For reasons I haven't looked into, these headers fail to compile when
+// included after the glog header. Prevent clang-format from ordering them.
+// clang-format off
#include "third_party/cddlib/lib-src/setoper.h"
#include "third_party/cddlib/lib-src/cdd.h"
+// clang-format on
#include "glog/logging.h"
#endif // __linux__
@@ -64,11 +68,14 @@
class HVPolytope : public Polytope<number_of_dimensions, Scalar> {
public:
// Constructs a polytope given the H and k matrices.
- HVPolytope(Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints,
- number_of_dimensions>> H,
- Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> k,
- Eigen::Ref<const Eigen::Matrix<Scalar, number_of_dimensions,
- num_vertices>> vertices)
+ HVPolytope(
+ Eigen::Ref<
+ const Eigen::Matrix<Scalar, num_constraints, number_of_dimensions>>
+ H,
+ Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> k,
+ Eigen::Ref<
+ const Eigen::Matrix<Scalar, number_of_dimensions, num_vertices>>
+ vertices)
: H_(H), k_(k), vertices_(vertices) {}
Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, number_of_dimensions>>
@@ -81,8 +88,7 @@
return H_;
}
- Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> static_k()
- const {
+ Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> static_k() const {
return k_;
}
Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k()
@@ -95,8 +101,8 @@
return vertices_;
}
- Eigen::Matrix<Scalar, number_of_dimensions, num_vertices>
- StaticVertices() const {
+ Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> StaticVertices()
+ const {
return vertices_;
}
@@ -106,18 +112,16 @@
const Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> vertices_;
};
-
-
#ifdef __linux__
template <int number_of_dimensions>
class HPolytope : public Polytope<number_of_dimensions> {
public:
// Constructs a polytope given the H and k matrices.
- HPolytope(
- Eigen::Ref<
- const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> H,
- Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k)
+ HPolytope(Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic,
+ number_of_dimensions>>
+ H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k)
: H_(H), k_(k), vertices_(CalculateVertices(H, k)) {}
// This is an initialization function shared across all instantiations of this
diff --git a/frc971/control_loops/polytope_test.cc b/frc971/control_loops/polytope_test.cc
index 849438d..0a01248 100644
--- a/frc971/control_loops/polytope_test.cc
+++ b/frc971/control_loops/polytope_test.cc
@@ -3,8 +3,8 @@
#include <vector>
#include "Eigen/Dense"
-#include "gtest/gtest.h"
#include "gmock/gmock.h"
+#include "gtest/gtest.h"
#include "aos/testing/test_logging.h"
@@ -77,25 +77,29 @@
// Tests that the vertices for various polytopes calculated from H and k are
// correct.
TEST_F(HPolytopeTest, CalculatedVertices) {
- EXPECT_THAT(MatrixToVectors(Polytope1().Vertices()),
- ::testing::UnorderedElementsAreArray(
- MatrixToVectors((Eigen::Matrix<double, 2, 4>() << -12, -12,
- 12, 12, -12, 12, 12, -12).finished())));
- EXPECT_THAT(MatrixToVectors(Polytope2().Vertices()),
- ::testing::UnorderedElementsAreArray(
- MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 24, 0, -24,
- 0, -12, 12, 12, -12).finished())));
+ EXPECT_THAT(
+ MatrixToVectors(Polytope1().Vertices()),
+ ::testing::UnorderedElementsAreArray(MatrixToVectors(
+ (Eigen::Matrix<double, 2, 4>() << -12, -12, 12, 12, -12, 12, 12, -12)
+ .finished())));
+ EXPECT_THAT(
+ MatrixToVectors(Polytope2().Vertices()),
+ ::testing::UnorderedElementsAreArray(MatrixToVectors(
+ (Eigen::Matrix<double, 2, 4>() << 24, 0, -24, 0, -12, 12, 12, -12)
+ .finished())));
EXPECT_THAT(MatrixToVectors(Polytope3().Vertices()),
::testing::UnorderedElementsAreArray(MatrixToVectors(
(Eigen::Matrix<double, 1, 2>() << 5, -4).finished())));
- EXPECT_THAT(MatrixToVectors(Polytope4().Vertices()),
- ::testing::UnorderedElementsAreArray(
- MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 1, 1.5, 1.5,
- 2, 0, -0.5, 0.5, 0).finished())));
- EXPECT_THAT(MatrixToVectors(Polytope5().Vertices()),
- ::testing::UnorderedElementsAreArray(
- MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 0.5, 1, 1.5,
- 1, 0, 0.5, 0, -0.5).finished())));
+ EXPECT_THAT(
+ MatrixToVectors(Polytope4().Vertices()),
+ ::testing::UnorderedElementsAreArray(MatrixToVectors(
+ (Eigen::Matrix<double, 2, 4>() << 1, 1.5, 1.5, 2, 0, -0.5, 0.5, 0)
+ .finished())));
+ EXPECT_THAT(
+ MatrixToVectors(Polytope5().Vertices()),
+ ::testing::UnorderedElementsAreArray(MatrixToVectors(
+ (Eigen::Matrix<double, 2, 4>() << 0.5, 1, 1.5, 1, 0, 0.5, 0, -0.5)
+ .finished())));
}
} // namespace controls
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 7945e5e..1eccf28 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -4,6 +4,7 @@
#include <vector>
#include "Eigen/Dense"
+
#include "aos/util/math.h"
namespace frc971 {
@@ -158,7 +159,7 @@
// Position and yaw relative to base_.
Pos pos_;
Scalar theta_;
-}; // class TypedPose
+}; // class TypedPose
typedef TypedPose<double> Pose;
@@ -222,6 +223,7 @@
::std::vector<TypedPose<Scalar>> PlotPoints() const {
return {pose1_, pose2_};
}
+
private:
TypedPose<Scalar> pose1_;
TypedPose<Scalar> pose2_;
diff --git a/frc971/control_loops/pose_test.cc b/frc971/control_loops/pose_test.cc
index 18486e3..845dda7 100644
--- a/frc971/control_loops/pose_test.cc
+++ b/frc971/control_loops/pose_test.cc
@@ -97,8 +97,7 @@
pose.set_theta(M_PI_2);
TransformationMatrix expected;
expected << 0, -1, 0, 1, 1, 0, 0, 2, 0, 0, 1, 3, 0, 0, 0, 1;
- TransformationMatrix pose_transformation =
- pose.AsTransformationMatrix();
+ TransformationMatrix pose_transformation = pose.AsTransformationMatrix();
ASSERT_LT((expected - pose_transformation).norm(), 1e-15)
<< "expected:\n"
<< expected << "\nBut got:\n"
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 77e2d5f..fae0e7d 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -1,8 +1,9 @@
#ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
#define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
-#include "aos/testing/random_seed.h"
#include "flatbuffers/flatbuffers.h"
+
+#include "aos/testing/random_seed.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/gaussian_noise.h"
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index e8beabf..4be0dc4 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -1,15 +1,15 @@
+#include "frc971/control_loops/position_sensor_sim.h"
+
#include <unistd.h>
#include <memory>
-
#include <random>
+#include "flatbuffers/flatbuffers.h"
+#include "gtest/gtest.h"
+
#include "aos/die.h"
#include "frc971/control_loops/control_loops_generated.h"
-#include "frc971/control_loops/position_sensor_sim.h"
-#include "gtest/gtest.h"
-
-#include "flatbuffers/flatbuffers.h"
namespace frc971 {
namespace control_loops {
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 1343770..dff5aad 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -7,6 +7,7 @@
#include <utility>
#include "Eigen/Dense"
+
#include "aos/util/trapezoid_profile.h"
#include "frc971/constants.h"
#include "frc971/control_loops/control_loop.h"
diff --git a/frc971/control_loops/quaternion_utils_test.cc b/frc971/control_loops/quaternion_utils_test.cc
index ada5e60..32b1915 100644
--- a/frc971/control_loops/quaternion_utils_test.cc
+++ b/frc971/control_loops/quaternion_utils_test.cc
@@ -1,14 +1,15 @@
-#include "Eigen/Dense"
+#include "frc971/control_loops/quaternion_utils.h"
#include <random>
-#include "aos/testing/random_seed.h"
-#include "frc971/control_loops/jacobian.h"
-#include "frc971/control_loops/quaternion_utils.h"
-#include "frc971/control_loops/runge_kutta.h"
+#include "Eigen/Dense"
#include "glog/logging.h"
#include "gtest/gtest.h"
+#include "aos/testing/random_seed.h"
+#include "frc971/control_loops/jacobian.h"
+#include "frc971/control_loops/runge_kutta.h"
+
namespace frc971 {
namespace controls {
namespace testing {
diff --git a/frc971/control_loops/simple_capped_state_feedback_loop.h b/frc971/control_loops/simple_capped_state_feedback_loop.h
index 9ed40d6..b2049a6 100644
--- a/frc971/control_loops/simple_capped_state_feedback_loop.h
+++ b/frc971/control_loops/simple_capped_state_feedback_loop.h
@@ -15,8 +15,9 @@
: public StateFeedbackLoop<number_of_states, number_of_inputs,
number_of_outputs> {
public:
- SimpleCappedStateFeedbackLoop(StateFeedbackLoop<
- number_of_states, number_of_inputs, number_of_outputs> &&loop)
+ SimpleCappedStateFeedbackLoop(
+ StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs>
+ &&loop)
: StateFeedbackLoop<number_of_states, number_of_inputs,
number_of_outputs>(::std::move(loop)),
min_voltages_(
@@ -66,8 +67,7 @@
private:
void CapU() override {
- this->mutable_U() =
- this->U().array().min(max_voltages_).max(min_voltages_);
+ this->mutable_U() = this->U().array().min(max_voltages_).max(min_voltages_);
}
::Eigen::Array<double, number_of_inputs, 1> min_voltages_;
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 1ea94d9..65e0ba1 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -8,11 +8,13 @@
#include <vector>
#include "Eigen/Dense"
+
#include "unsupported/Eigen/MatrixFunctions"
#if defined(__linux__)
-#include "aos/logging/logging.h"
#include "glog/logging.h"
+
+#include "aos/logging/logging.h"
#endif
#include "aos/macros.h"
@@ -356,7 +358,8 @@
&&observers)
: coefficients_(::std::move(observers)) {
last_U_ = Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic>(
- number_of_inputs, std::max(static_cast<size_t>(1u), coefficients().delayed_u));
+ number_of_inputs,
+ std::max(static_cast<size_t>(1u), coefficients().delayed_u));
}
StateFeedbackObserver(StateFeedbackObserver &&other)
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 62e3d08..c886775 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -1,6 +1,9 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "flatbuffers/flatbuffers.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loop_test.h"
@@ -14,8 +17,6 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_goal_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_subsystem_output_generated.h"
#include "frc971/zeroing/zeroing.h"
-#include "glog/logging.h"
-#include "gtest/gtest.h"
using ::frc971::control_loops::PositionSensorSimulator;
diff --git a/frc971/control_loops/voltage_cap/voltage_cap.cc b/frc971/control_loops/voltage_cap/voltage_cap.cc
index 3de5828..d6d8456 100644
--- a/frc971/control_loops/voltage_cap/voltage_cap.cc
+++ b/frc971/control_loops/voltage_cap/voltage_cap.cc
@@ -1,4 +1,4 @@
-#include "voltage_cap.h"
+#include "frc971/control_loops/voltage_cap/voltage_cap.h"
#include <limits>
diff --git a/frc971/control_loops/voltage_cap/voltage_cap_test.cc b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
index 4eab267..ea812b9 100644
--- a/frc971/control_loops/voltage_cap/voltage_cap_test.cc
+++ b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
@@ -1,7 +1,7 @@
-#include <unistd.h>
-
#include "frc971/control_loops/voltage_cap/voltage_cap.h"
+#include <unistd.h>
+
#include "gtest/gtest.h"
namespace frc971 {
@@ -103,7 +103,7 @@
EXPECT_EQ(12.0, voltage_one);
EXPECT_EQ(-10.0, voltage_two);
}
-TEST_F(VoltageTest, QuadrantOneToFour12){
+TEST_F(VoltageTest, QuadrantOneToFour12) {
// Point in Quadrant 1 intersects box in Quadrant 4.
double voltage_one, voltage_two;
VoltageCap(12.0, 11.0, 33.0, &voltage_one, &voltage_two);
@@ -218,7 +218,7 @@
EXPECT_EQ(6.0, voltage_one);
EXPECT_EQ(-5.0, voltage_two);
}
-TEST_F(VoltageTest, QuadrantOneToFour6){
+TEST_F(VoltageTest, QuadrantOneToFour6) {
// Point in Quadrant 1 intersects box in Quadrant 4.
double voltage_one, voltage_two;
VoltageCap(6.0, 22.0, 33.0, &voltage_one, &voltage_two);
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 449cee1..c94bb23 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -11,19 +11,19 @@
namespace testing {
class WristTest_NoWindupPositive_Test;
class WristTest_NoWindupNegative_Test;
-};
+}; // namespace testing
// Note: Everything in this file assumes that there is a 1 cycle delay between
// power being requested and it showing up at the motor. It assumes that
// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
// that isn't true.
-template<int kNumZeroSensors>
+template <int kNumZeroSensors>
class ZeroedJoint;
// This class implements the CapU function correctly given all the extra
// information that we know about from the wrist motor.
-template<int kNumZeroSensors>
+template <int kNumZeroSensors>
class ZeroedStateFeedbackLoop : public StateFeedbackLoop<3, 1, 1> {
public:
ZeroedStateFeedbackLoop(StateFeedbackLoop<3, 1, 1> loop,
@@ -31,8 +31,7 @@
: StateFeedbackLoop<3, 1, 1>(loop),
zeroed_joint_(zeroed_joint),
voltage_(0.0),
- last_voltage_(0.0) {
- }
+ last_voltage_(0.0) {}
// Caps U, but this time respects the state of the wrist as well.
virtual void CapU();
@@ -45,6 +44,7 @@
// Zeros the accumulator.
void ZeroPower() { voltage_ = 0.0; }
+
private:
ZeroedJoint<kNumZeroSensors> *zeroed_joint_;
@@ -54,7 +54,7 @@
double uncapped_voltage_;
};
-template<int kNumZeroSensors>
+template <int kNumZeroSensors>
void ZeroedStateFeedbackLoop<kNumZeroSensors>::CapU() {
const double old_voltage = voltage_;
voltage_ += U(0, 0);
@@ -74,18 +74,18 @@
const bool is_ready =
zeroed_joint_->state_ == ZeroedJoint<kNumZeroSensors>::READY;
- double limit = is_ready ?
- 12.0 : zeroed_joint_->config_data_.max_zeroing_voltage;
+ double limit =
+ is_ready ? 12.0 : zeroed_joint_->config_data_.max_zeroing_voltage;
// Make sure that reality and the observer can't get too far off. There is a
// delay by one cycle between the applied voltage and X_hat(2, 0), so compare
// against last cycle's voltage.
if (X_hat(2, 0) > last_voltage_ + 2.0) {
- //X_hat(2, 0) = last_voltage_ + 2.0;
+ // X_hat(2, 0) = last_voltage_ + 2.0;
voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
- } else if (X_hat(2, 0) < last_voltage_ -2.0) {
- //X_hat(2, 0) = last_voltage_ - 2.0;
+ } else if (X_hat(2, 0) < last_voltage_ - 2.0) {
+ // X_hat(2, 0) = last_voltage_ - 2.0;
voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
}
@@ -99,10 +99,9 @@
last_voltage_ = voltage_;
}
-
// Class to zero and control a joint with any number of zeroing sensors with a
// state feedback controller.
-template<int kNumZeroSensors>
+template <int kNumZeroSensors>
class ZeroedJoint {
public:
// Sturcture to hold the hardware configuration information.
@@ -229,7 +228,7 @@
MOVING_OFF,
ZEROING,
READY,
- ESTOP
+ ESTOP,
};
// Internal state for zeroing.
@@ -362,7 +361,7 @@
// again.
const double calibration =
position->hall_effect_positions[active_sensor_index];
- if (!is_between(last_off_position_, position->position,
+ if (!is_between(last_off_position_, position->position,
calibration)) {
LOG(ERROR, "Got a bogus calibration number. Trying again.\n");
LOG(ERROR,
@@ -405,8 +404,8 @@
// Update the observer.
loop_->Update(position != NULL, !output_enabled);
- LOG(DEBUG, "X_hat={%f, %f, %f}\n",
- loop_->X_hat(0, 0), loop_->X_hat(1, 0), loop_->X_hat(2, 0));
+ LOG(DEBUG, "X_hat={%f, %f, %f}\n", loop_->X_hat(0, 0), loop_->X_hat(1, 0),
+ loop_->X_hat(2, 0));
capped_goal_ = false;
// Verify that the zeroing goal hasn't run away.
@@ -420,13 +419,16 @@
case ZEROING:
// Check if we have cliped and adjust the goal.
if (loop_->uncapped_voltage() > config_data_.max_zeroing_voltage) {
- double dx = (loop_->uncapped_voltage() -
- config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ double dx =
+ (loop_->uncapped_voltage() - config_data_.max_zeroing_voltage) /
+ loop_->K(0, 0);
zeroing_position_ -= dx;
capped_goal_ = true;
- } else if(loop_->uncapped_voltage() < -config_data_.max_zeroing_voltage) {
- double dx = (loop_->uncapped_voltage() +
- config_data_.max_zeroing_voltage) / loop_->K(0, 0);
+ } else if (loop_->uncapped_voltage() <
+ -config_data_.max_zeroing_voltage) {
+ double dx =
+ (loop_->uncapped_voltage() + config_data_.max_zeroing_voltage) /
+ loop_->K(0, 0);
zeroing_position_ -= dx;
capped_goal_ = true;
}
diff --git a/frc971/image_streamer/image_streamer.cc b/frc971/image_streamer/image_streamer.cc
index de56d55..7f85b3f 100644
--- a/frc971/image_streamer/image_streamer.cc
+++ b/frc971/image_streamer/image_streamer.cc
@@ -15,15 +15,16 @@
#include <thread>
#include "absl/strings/str_format.h"
+#include "flatbuffers/flatbuffers.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
#include "aos/events/glib_main_loop.h"
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/network/web_proxy_generated.h"
#include "aos/seasocks/seasocks_logger.h"
-#include "flatbuffers/flatbuffers.h"
#include "frc971/vision/vision_generated.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
#include "internal/Embedded.h"
#include "seasocks/Server.h"
#include "seasocks/StringUtil.h"
diff --git a/frc971/imu/blink.c b/frc971/imu/blink.c
index d478d78..f34552b 100644
--- a/frc971/imu/blink.c
+++ b/frc971/imu/blink.c
@@ -10,14 +10,14 @@
#ifndef PICO_DEFAULT_LED_PIN
#warning blink example requires a board with a regular LED
#else
- const uint LED_PIN = PICO_DEFAULT_LED_PIN;
- gpio_init(LED_PIN);
- gpio_set_dir(LED_PIN, GPIO_OUT);
- while (true) {
- gpio_put(LED_PIN, 1);
- sleep_ms(250);
- gpio_put(LED_PIN, 0);
- sleep_ms(250);
- }
+ const uint LED_PIN = PICO_DEFAULT_LED_PIN;
+ gpio_init(LED_PIN);
+ gpio_set_dir(LED_PIN, GPIO_OUT);
+ while (true) {
+ gpio_put(LED_PIN, 1);
+ sleep_ms(250);
+ gpio_put(LED_PIN, 0);
+ sleep_ms(250);
+ }
#endif
}
diff --git a/frc971/imu_reader/imu.cc b/frc971/imu_reader/imu.cc
index b477e4c..acdd6d9 100644
--- a/frc971/imu_reader/imu.cc
+++ b/frc971/imu_reader/imu.cc
@@ -1,8 +1,9 @@
#include "frc971/imu_reader/imu.h"
-#include "aos/util/crc32.h"
#include "glog/logging.h"
+#include "aos/util/crc32.h"
+
namespace frc971::imu {
namespace {
diff --git a/frc971/shooter_interpolation/interpolation.h b/frc971/shooter_interpolation/interpolation.h
index dd0e316..bce532f 100644
--- a/frc971/shooter_interpolation/interpolation.h
+++ b/frc971/shooter_interpolation/interpolation.h
@@ -21,7 +21,7 @@
// power for a shot
YValue Get(double x) const;
- bool GetInRange(double x, YValue* type) const;
+ bool GetInRange(double x, YValue *type) const;
private:
// Contains the list of angle entries in the interpolation table
@@ -29,21 +29,20 @@
};
template <typename YValue>
-InterpolationTable<YValue>::InterpolationTable(const ::std::vector<Point> &table)
- : table_(table) {
- ::std::sort(table_.begin(), table_.end(),
- [](const Point &a, const Point &b) {
- return a.first < b.first;
- });
- }
+InterpolationTable<YValue>::InterpolationTable(
+ const ::std::vector<Point> &table)
+ : table_(table) {
+ ::std::sort(table_.begin(), table_.end(),
+ [](const Point &a, const Point &b) { return a.first < b.first; });
+}
template <typename YValue>
YValue InterpolationTable<YValue>::Get(double x) const {
// Points to to the smallest item such that it->first >= dist, or end() if no
// such item exists.
- auto it = ::std::lower_bound(table_.begin(), table_.end(), x,
- [](const Point &a,
- double dist) { return a.first < dist; });
+ auto it = ::std::lower_bound(
+ table_.begin(), table_.end(), x,
+ [](const Point &a, double dist) { return a.first < dist; });
if (it == table_.begin()) {
return it->second;
} else if (it == table_.end()) {
@@ -59,12 +58,12 @@
}
template <typename YValue>
-bool InterpolationTable<YValue>::GetInRange(double x, YValue* result) const {
+bool InterpolationTable<YValue>::GetInRange(double x, YValue *result) const {
// Points to to the smallest item such that it->first >= dist, or end() if no
// such item exists.
- auto it = ::std::lower_bound(table_.begin(), table_.end(), x,
- [](const Point &a,
- double dist) { return a.first < dist; });
+ auto it = ::std::lower_bound(
+ table_.begin(), table_.end(), x,
+ [](const Point &a, double dist) { return a.first < dist; });
if (it == table_.begin()) {
return false;
} else if (it == table_.end()) {
diff --git a/frc971/shooter_interpolation/interpolation_test.cc b/frc971/shooter_interpolation/interpolation_test.cc
index fd99124..92a2b76 100644
--- a/frc971/shooter_interpolation/interpolation_test.cc
+++ b/frc971/shooter_interpolation/interpolation_test.cc
@@ -1,3 +1,5 @@
+#include "frc971/shooter_interpolation/interpolation.h"
+
#include <unistd.h>
#include <memory>
@@ -6,19 +8,16 @@
#include "gtest/gtest.h"
-#include "frc971/shooter_interpolation/interpolation.h"
-
namespace frc971 {
namespace shooter_interpolation {
struct TestShotParams {
double angle;
double power;
- static TestShotParams BlendY(double x, const TestShotParams& a, const TestShotParams& b) {
- return TestShotParams{
- Blend(x, a.angle, b.angle),
- Blend(x, a.power, b.power)
- };
+ static TestShotParams BlendY(double x, const TestShotParams &a,
+ const TestShotParams &b) {
+ return TestShotParams{Blend(x, a.angle, b.angle),
+ Blend(x, a.power, b.power)};
}
};
@@ -32,7 +31,10 @@
// correctly
TEST(InterpolationTable, ExactNumbers) {
::std::vector<::std::pair<double, TestShotParams>> data = {
- {1, {10, 10}}, {3, {20, 20}}, {2, {15, 12345678}}, {4, {10, 567.323}},
+ {1, {10, 10}},
+ {3, {20, 20}},
+ {2, {15, 12345678}},
+ {4, {10, 567.323}},
};
TestInterpolationTable interpolation(data);
@@ -44,7 +46,10 @@
// correctly
TEST(InterpolationTable, InexactNumbers) {
::std::vector<::std::pair<double, TestShotParams>> data = {
- {1, {10, 10}}, {3, {20, 20}}, {2, {15, 15}}, {4, {10, 567.323}},
+ {1, {10, 10}},
+ {3, {20, 20}},
+ {2, {15, 15}},
+ {4, {10, 567.323}},
};
TestInterpolationTable interpolation(data);
@@ -56,7 +61,10 @@
// processed correctly
TEST(InterpolationTable, OutOfScopeNumbers) {
::std::vector<::std::pair<double, TestShotParams>> data = {
- {1, {10, 10}}, {3, {20, 20}}, {2, {15, 12345678}}, {4, {10, 567.323}},
+ {1, {10, 10}},
+ {3, {20, 20}},
+ {2, {15, 12345678}},
+ {4, {10, 567.323}},
};
TestInterpolationTable interpolation(data);
diff --git a/frc971/solvers/convex.h b/frc971/solvers/convex.h
index 140cac1..3de357c 100644
--- a/frc971/solvers/convex.h
+++ b/frc971/solvers/convex.h
@@ -4,11 +4,11 @@
#include <sys/types.h>
#include <unistd.h>
-#include <Eigen/Dense>
#include <iomanip>
#include "absl/strings/str_join.h"
#include "glog/logging.h"
+#include <Eigen/Dense>
namespace frc971 {
namespace solvers {
diff --git a/frc971/solvers/sparse_convex.cc b/frc971/solvers/sparse_convex.cc
index dfb418f..7e2a6ca 100644
--- a/frc971/solvers/sparse_convex.cc
+++ b/frc971/solvers/sparse_convex.cc
@@ -1,10 +1,9 @@
#include "frc971/solvers/sparse_convex.h"
-#include <Eigen/Sparse>
-#include <Eigen/SparseLU>
-
#include "absl/strings/str_join.h"
#include "glog/logging.h"
+#include <Eigen/Sparse>
+#include <Eigen/SparseLU>
namespace frc971 {
namespace solvers {
diff --git a/frc971/solvers/sparse_convex.h b/frc971/solvers/sparse_convex.h
index 87f9bcb..9695431 100644
--- a/frc971/solvers/sparse_convex.h
+++ b/frc971/solvers/sparse_convex.h
@@ -4,10 +4,10 @@
#include <sys/types.h>
#include <unistd.h>
-#include <Eigen/Sparse>
#include <iomanip>
#include "glog/logging.h"
+#include <Eigen/Sparse>
namespace frc971 {
namespace solvers {
@@ -104,21 +104,18 @@
};
// Computes all the values for the given problem at the given state.
- Derivatives ComputeDerivative(
- const SparseConvexProblem &problem,
- const Eigen::Ref<const Eigen::VectorXd> y);
+ Derivatives ComputeDerivative(const SparseConvexProblem &problem,
+ const Eigen::Ref<const Eigen::VectorXd> y);
// Computes Rt at the given state and with the given t_inverse. See 11.53 of
// cvxbook.pdf.
- Eigen::VectorXd Rt(
- const Derivatives &derivatives,
- Eigen::VectorXd y, double t_inverse);
+ Eigen::VectorXd Rt(const Derivatives &derivatives, Eigen::VectorXd y,
+ double t_inverse);
// Prints out all the derivatives with VLOG at the provided verbosity.
- void PrintDerivatives(
- const Derivatives &derivatives,
- const Eigen::Ref<const Eigen::VectorXd> y,
- std::string_view prefix, int verbosity);
+ void PrintDerivatives(const Derivatives &derivatives,
+ const Eigen::Ref<const Eigen::VectorXd> y,
+ std::string_view prefix, int verbosity);
};
} // namespace solvers
diff --git a/frc971/solvers/sparse_convex_test.cc b/frc971/solvers/sparse_convex_test.cc
index e391aa4..63a9409 100644
--- a/frc971/solvers/sparse_convex_test.cc
+++ b/frc971/solvers/sparse_convex_test.cc
@@ -36,8 +36,7 @@
}
// Returns the constraints f(X) < 0, and their derivitive.
- Eigen::VectorXd f(
- Eigen::Ref<const Eigen::VectorXd> X) const override {
+ Eigen::VectorXd f(Eigen::Ref<const Eigen::VectorXd> X) const override {
return C_ * X - c_;
}
Eigen::SparseMatrix<double> df(
@@ -49,9 +48,7 @@
Eigen::SparseMatrix<double> A() const override {
return Eigen::Matrix<double, 1, 2>(1, -1).sparseView();
}
- Eigen::VectorXd b() const override {
- return Eigen::Matrix<double, 1, 1>(0);
- }
+ Eigen::VectorXd b() const override { return Eigen::Matrix<double, 1, 1>(0); }
private:
Eigen::Matrix<double, 2, 2> Q_;
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index f01af3c..3b0bcc6 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -2,14 +2,15 @@
#include <algorithm>
#include <limits>
-#include <opencv2/highgui/highgui.hpp>
#include "Eigen/Dense"
+#include "external/com_github_foxglove_schemas/CompressedImage_schema.h"
+#include "external/com_github_foxglove_schemas/ImageAnnotations_schema.h"
+#include <opencv2/highgui/highgui.hpp>
+
#include "aos/events/simulated_event_loop.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
-#include "external/com_github_foxglove_schemas/CompressedImage_schema.h"
-#include "external/com_github_foxglove_schemas/ImageAnnotations_schema.h"
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/charuco_lib.h"
#include "frc971/wpilib/imu_batch_generated.h"
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index 132bb4b..6451d0c 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -4,6 +4,7 @@
#include <vector>
#include "Eigen/Dense"
+
#include "aos/events/simulated_event_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/quaternion_utils.h"
diff --git a/frc971/vision/ceres/pose_graph_3d_error_term.h b/frc971/vision/ceres/pose_graph_3d_error_term.h
index c2ebde7..4dd5fbd 100644
--- a/frc971/vision/ceres/pose_graph_3d_error_term.h
+++ b/frc971/vision/ceres/pose_graph_3d_error_term.h
@@ -33,6 +33,7 @@
#include "Eigen/Core"
#include "ceres/autodiff_cost_function.h"
+
#include "types.h"
namespace ceres {
diff --git a/frc971/vision/ceres/read_g2o.h b/frc971/vision/ceres/read_g2o.h
index fea32e9..69fa16a 100644
--- a/frc971/vision/ceres/read_g2o.h
+++ b/frc971/vision/ceres/read_g2o.h
@@ -44,8 +44,8 @@
// Reads a single pose from the input and inserts it into the map. Returns false
// if there is a duplicate entry.
template <typename Pose, typename Allocator>
-bool ReadVertex(std::ifstream* infile,
- std::map<int, Pose, std::less<int>, Allocator>* poses) {
+bool ReadVertex(std::ifstream *infile,
+ std::map<int, Pose, std::less<int>, Allocator> *poses) {
int id;
Pose pose;
*infile >> id >> pose;
@@ -62,8 +62,8 @@
// Reads the contraints between two vertices in the pose graph
template <typename Constraint, typename Allocator>
-void ReadConstraint(std::ifstream* infile,
- std::vector<Constraint, Allocator>* constraints) {
+void ReadConstraint(std::ifstream *infile,
+ std::vector<Constraint, Allocator> *constraints) {
Constraint constraint;
*infile >> constraint;
@@ -92,18 +92,17 @@
// where the quaternion is in Hamilton form.
// A constraint is defined as follows:
//
-// EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT
+// EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12
+// I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT
//
// where I_ij is the (i, j)-th entry of the information matrix for the
// measurement. Only the upper-triangular part is stored. The measurement order
// is the delta position followed by the delta orientation.
-template <typename Pose,
- typename Constraint,
- typename MapAllocator,
+template <typename Pose, typename Constraint, typename MapAllocator,
typename VectorAllocator>
-bool ReadG2oFile(const std::string& filename,
- std::map<int, Pose, std::less<int>, MapAllocator>* poses,
- std::vector<Constraint, VectorAllocator>* constraints) {
+bool ReadG2oFile(const std::string &filename,
+ std::map<int, Pose, std::less<int>, MapAllocator> *poses,
+ std::vector<Constraint, VectorAllocator> *constraints) {
CHECK(poses != NULL);
CHECK(constraints != NULL);
diff --git a/frc971/vision/ceres/types.h b/frc971/vision/ceres/types.h
index d3f19ed..17e9bd3 100644
--- a/frc971/vision/ceres/types.h
+++ b/frc971/vision/ceres/types.h
@@ -52,7 +52,7 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-inline std::istream& operator>>(std::istream& input, Pose3d& pose) {
+inline std::istream &operator>>(std::istream &input, Pose3d &pose) {
input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >> pose.q.y() >>
pose.q.z() >> pose.q.w();
// Normalize the quaternion to account for precision loss due to
@@ -61,9 +61,7 @@
return input;
}
-typedef std::map<int,
- Pose3d,
- std::less<int>,
+typedef std::map<int, Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d>>>
MapOfPoses;
@@ -88,8 +86,8 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-inline std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
- Pose3d& t_be = constraint.t_be;
+inline std::istream &operator>>(std::istream &input, Constraint3d &constraint) {
+ Pose3d &t_be = constraint.t_be;
input >> constraint.id_begin >> constraint.id_end >> t_be;
for (int i = 0; i < 6 && input.good(); ++i) {
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 89ebe19..b55a493 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -2,17 +2,18 @@
#include <chrono>
#include <functional>
+#include <string_view>
+
+#include "glog/logging.h"
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
-#include <string_view>
#include "aos/events/event_loop.h"
#include "aos/flatbuffers.h"
#include "aos/network/team_number.h"
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/vision_generated.h"
-#include "glog/logging.h"
DEFINE_string(board_template_path, "",
"If specified, write an image to the specified path for the "
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 2c35a0b..8af7b8c 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -2,16 +2,17 @@
#define Y2020_VISION_CHARUCO_LIB_H_
#include <functional>
-#include <opencv2/aruco/charuco.hpp>
-#include <opencv2/calib3d.hpp>
#include <string_view>
#include "Eigen/Dense"
#include "Eigen/Geometry"
#include "absl/types/span.h"
+#include "external/com_github_foxglove_schemas/ImageAnnotations_generated.h"
+#include <opencv2/aruco/charuco.hpp>
+#include <opencv2/calib3d.hpp>
+
#include "aos/events/event_loop.h"
#include "aos/network/message_bridge_server_generated.h"
-#include "external/com_github_foxglove_schemas/ImageAnnotations_generated.h"
#include "frc971/vision/calibration_generated.h"
DECLARE_bool(visualize);
diff --git a/frc971/vision/extrinsics_calibration.cc b/frc971/vision/extrinsics_calibration.cc
index daa371e..9459c84 100644
--- a/frc971/vision/extrinsics_calibration.cc
+++ b/frc971/vision/extrinsics_calibration.cc
@@ -1,19 +1,19 @@
#include "frc971/vision/extrinsics_calibration.h"
-#include "aos/time/time.h"
#include "ceres/ceres.h"
-#include "frc971/analysis/in_process_plotter.h"
-#include "frc971/control_loops/runge_kutta.h"
-#include "frc971/vision/calibration_accumulator.h"
-#include "frc971/vision/charuco_lib.h"
-#include "frc971/vision/visualize_robot.h"
-
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
+#include "aos/time/time.h"
+#include "frc971/analysis/in_process_plotter.h"
+#include "frc971/control_loops/runge_kutta.h"
+#include "frc971/vision/calibration_accumulator.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/visualize_robot.h"
+
namespace frc971 {
namespace vision {
diff --git a/frc971/vision/extrinsics_calibration.h b/frc971/vision/extrinsics_calibration.h
index fb52ed9..f6c3ccc 100644
--- a/frc971/vision/extrinsics_calibration.h
+++ b/frc971/vision/extrinsics_calibration.h
@@ -3,6 +3,7 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
+
#include "frc971/vision/calibration_accumulator.h"
namespace frc971 {
diff --git a/frc971/vision/foxglove_image_converter_lib.h b/frc971/vision/foxglove_image_converter_lib.h
index 872ac14..cdba9e3 100644
--- a/frc971/vision/foxglove_image_converter_lib.h
+++ b/frc971/vision/foxglove_image_converter_lib.h
@@ -1,7 +1,8 @@
#ifndef FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_
#define FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_
-#include "aos/events/event_loop.h"
#include "external/com_github_foxglove_schemas/CompressedImage_generated.h"
+
+#include "aos/events/event_loop.h"
#include "frc971/vision/charuco_lib.h"
#include "frc971/vision/vision_generated.h"
diff --git a/frc971/vision/foxglove_image_converter_test.cc b/frc971/vision/foxglove_image_converter_test.cc
index 7a3f786..c9c7d66 100644
--- a/frc971/vision/foxglove_image_converter_test.cc
+++ b/frc971/vision/foxglove_image_converter_test.cc
@@ -1,10 +1,10 @@
-#include "frc971/vision/foxglove_image_converter_lib.h"
+#include "gtest/gtest.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/testing/path.h"
#include "aos/testing/tmpdir.h"
-#include "gtest/gtest.h"
+#include "frc971/vision/foxglove_image_converter_lib.h"
DECLARE_int32(jpeg_quality);
diff --git a/frc971/vision/geometry.h b/frc971/vision/geometry.h
index e3527f5..7858418 100644
--- a/frc971/vision/geometry.h
+++ b/frc971/vision/geometry.h
@@ -1,10 +1,11 @@
#ifndef FRC971_VISION_GEOMETRY_H_
#define FRC971_VISION_GEOMETRY_H_
-#include "aos/util/math.h"
#include "glog/logging.h"
#include "opencv2/core/types.hpp"
+#include "aos/util/math.h"
+
namespace frc971::vision {
// Linear equation in the form y = mx + b
diff --git a/frc971/vision/geometry_test.cc b/frc971/vision/geometry_test.cc
index 19e54c4..f34594c 100644
--- a/frc971/vision/geometry_test.cc
+++ b/frc971/vision/geometry_test.cc
@@ -2,10 +2,11 @@
#include <cmath>
-#include "aos/util/math.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
+#include "aos/util/math.h"
+
namespace frc971::vision::testing {
TEST(GeometryTest, SlopeInterceptLine) {
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
index 224a37c..452dd74 100644
--- a/frc971/vision/intrinsics_calibration.cc
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -1,10 +1,11 @@
#include <cmath>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc.hpp>
#include <regex>
#include "absl/strings/str_format.h"
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/network/team_number.h"
diff --git a/frc971/vision/intrinsics_calibration_lib.h b/frc971/vision/intrinsics_calibration_lib.h
index 10062ba..2da33f1 100644
--- a/frc971/vision/intrinsics_calibration_lib.h
+++ b/frc971/vision/intrinsics_calibration_lib.h
@@ -1,14 +1,15 @@
#ifndef FRC971_VISION_CALIBRATION_LIB_H_
#define FRC971_VISION_CALIBRATION_LIB_H_
#include <cmath>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc.hpp>
#include <regex>
#include "Eigen/Dense"
#include "Eigen/Geometry"
#include "absl/strings/str_format.h"
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
#include "aos/events/event_loop.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
diff --git a/frc971/vision/media_device.cc b/frc971/vision/media_device.cc
index 4c227a8..a369cfb 100644
--- a/frc971/vision/media_device.cc
+++ b/frc971/vision/media_device.cc
@@ -15,9 +15,10 @@
#include "absl/strings/str_cat.h"
#include "absl/strings/str_split.h"
+#include "glog/logging.h"
+
#include "aos/scoped/scoped_fd.h"
#include "aos/util/file.h"
-#include "glog/logging.h"
namespace frc971 {
namespace vision {
@@ -263,8 +264,9 @@
format.format.quantization = V4L2_QUANTIZATION_DEFAULT;
format.format.xfer_func = V4L2_XFER_FUNC_DEFAULT;
- LOG(INFO) << "Setting " << entity()->name() << " pad " << index() << " format to "
- << width << "x" << height << " code 0x" << std::hex << code;
+ LOG(INFO) << "Setting " << entity()->name() << " pad " << index()
+ << " format to " << width << "x" << height << " code 0x" << std::hex
+ << code;
PCHECK(ioctl(fd, VIDIOC_SUBDEV_S_FMT, &format) == 0);
diff --git a/frc971/vision/media_device.h b/frc971/vision/media_device.h
index b3b93a6..a2a86fe 100644
--- a/frc971/vision/media_device.h
+++ b/frc971/vision/media_device.h
@@ -12,9 +12,10 @@
#include <string_view>
#include <vector>
-#include "aos/scoped/scoped_fd.h"
#include "glog/logging.h"
+#include "aos/scoped/scoped_fd.h"
+
namespace frc971 {
namespace vision {
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 21bd443..e7a6955 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -1,6 +1,7 @@
#include "frc971/vision/target_mapper.h"
#include "absl/strings/str_format.h"
+
#include "frc971/control_loops/control_loop.h"
#include "frc971/vision/ceres/pose_graph_3d_error_term.h"
#include "frc971/vision/geometry.h"
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index d10d6d8..cfefcc7 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -3,8 +3,9 @@
#include <unordered_map>
-#include "aos/events/simulated_event_loop.h"
#include "ceres/ceres.h"
+
+#include "aos/events/simulated_event_loop.h"
#include "frc971/vision/ceres/types.h"
#include "frc971/vision/target_map_generated.h"
#include "frc971/vision/visualize_robot.h"
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 9c86965..8c6d37d 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -2,12 +2,13 @@
#include <random>
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
#include "aos/events/simulated_event_loop.h"
#include "aos/testing/path.h"
#include "aos/testing/random_seed.h"
#include "aos/util/math.h"
-#include "glog/logging.h"
-#include "gtest/gtest.h"
DECLARE_int32(min_target_id);
DECLARE_int32(max_target_id);
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
index 8764024..6338ba5 100644
--- a/frc971/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -5,6 +5,8 @@
#include <string>
#include "absl/types/span.h"
+#include "glog/logging.h"
+
#include "aos/containers/ring_buffer.h"
#include "aos/events/epoll.h"
#include "aos/events/event_loop.h"
@@ -13,7 +15,6 @@
#include "aos/scoped/scoped_fd.h"
#include "aos/util/threaded_consumer.h"
#include "frc971/vision/vision_generated.h"
-#include "glog/logging.h"
namespace frc971 {
namespace vision {
diff --git a/frc971/vision/visualize_robot.cc b/frc971/vision/visualize_robot.cc
index 55932ef..0c7a536 100644
--- a/frc971/vision/visualize_robot.cc
+++ b/frc971/vision/visualize_robot.cc
@@ -1,12 +1,11 @@
#include "frc971/vision/visualize_robot.h"
+#include "glog/logging.h"
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
-#include "glog/logging.h"
-
namespace frc971 {
namespace vision {
diff --git a/frc971/vision/visualize_robot.h b/frc971/vision/visualize_robot.h
index 31d584d..c679dba 100644
--- a/frc971/vision/visualize_robot.h
+++ b/frc971/vision/visualize_robot.h
@@ -1,13 +1,12 @@
#ifndef FRC971_VISION_VISUALIZE_ROBOT_H_
#define FRC971_VISION_VISUALIZE_ROBOT_H_
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
-#include "Eigen/Dense"
-#include "Eigen/Geometry"
-
namespace frc971 {
namespace vision {
diff --git a/frc971/vision/visualize_robot_sample.cc b/frc971/vision/visualize_robot_sample.cc
index 59acba0..6ca7a9c 100644
--- a/frc971/vision/visualize_robot_sample.cc
+++ b/frc971/vision/visualize_robot_sample.cc
@@ -1,19 +1,18 @@
-#include "frc971/vision/visualize_robot.h"
-
-#include "aos/init.h"
-#include "aos/logging/logging.h"
-#include "glog/logging.h"
+#include <math.h>
#include "Eigen/Dense"
-
-#include <math.h>
+#include "glog/logging.h"
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
+
+#include "aos/init.h"
+#include "aos/logging/logging.h"
#include "aos/time/time.h"
+#include "frc971/vision/visualize_robot.h"
namespace frc971 {
namespace vision {
diff --git a/frc971/wpilib/ADIS16470.cc b/frc971/wpilib/ADIS16470.cc
index d43d2fb..95d5baf 100644
--- a/frc971/wpilib/ADIS16470.cc
+++ b/frc971/wpilib/ADIS16470.cc
@@ -2,9 +2,10 @@
#include <cinttypes>
+#include "glog/logging.h"
+
#include "aos/containers/sized_array.h"
#include "aos/time/time.h"
-#include "glog/logging.h"
#include "hal/HAL.h"
namespace frc971 {
diff --git a/frc971/wpilib/ahal/AnalogInput.cc b/frc971/wpilib/ahal/AnalogInput.cc
index 7482476..7574715 100644
--- a/frc971/wpilib/ahal/AnalogInput.cc
+++ b/frc971/wpilib/ahal/AnalogInput.cc
@@ -6,13 +6,13 @@
/*----------------------------------------------------------------------------*/
#include "frc971/wpilib/ahal/AnalogInput.h"
-#include "hal/AnalogInput.h"
#include <sstream>
+#include "frc971/wpilib/ahal/WPIErrors.h"
+#include "hal/AnalogInput.h"
#include "hal/HAL.h"
#include "hal/Ports.h"
-#include "frc971/wpilib/ahal/WPIErrors.h"
using namespace frc;
diff --git a/frc971/wpilib/ahal/AnalogInput.h b/frc971/wpilib/ahal/AnalogInput.h
index 4540bbe..c4a4e94 100644
--- a/frc971/wpilib/ahal/AnalogInput.h
+++ b/frc971/wpilib/ahal/AnalogInput.h
@@ -10,8 +10,8 @@
#include <memory>
#include <string>
-#include "hal/Types.h"
#include "frc971/wpilib/ahal/SensorBase.h"
+#include "hal/Types.h"
namespace frc {
diff --git a/frc971/wpilib/ahal/AnalogTrigger.cc b/frc971/wpilib/ahal/AnalogTrigger.cc
index bd63189..aeaf6ce 100644
--- a/frc971/wpilib/ahal/AnalogTrigger.cc
+++ b/frc971/wpilib/ahal/AnalogTrigger.cc
@@ -7,12 +7,12 @@
#include "frc971/wpilib/ahal/AnalogTrigger.h"
-#include <memory>
-#include <utility>
-
#include <hal/FRCUsageReporting.h>
#include <hal/HAL.h>
+#include <memory>
+#include <utility>
+
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/Base.h"
#include "frc971/wpilib/ahal/DutyCycle.h"
@@ -25,7 +25,7 @@
m_ownsAnalog = true;
}
-AnalogTrigger::AnalogTrigger(AnalogInput* input) {
+AnalogTrigger::AnalogTrigger(AnalogInput *input) {
m_analogInput = input;
int32_t status = 0;
m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &status);
@@ -40,7 +40,7 @@
HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
}
-AnalogTrigger::AnalogTrigger(DutyCycle* input) {
+AnalogTrigger::AnalogTrigger(DutyCycle *input) {
m_dutyCycle = input;
int32_t status = 0;
m_trigger = HAL_InitializeAnalogTriggerDutyCycle(input->m_handle, &status);
@@ -72,7 +72,7 @@
std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
}
-AnalogTrigger& AnalogTrigger::operator=(AnalogTrigger&& rhs) {
+AnalogTrigger &AnalogTrigger::operator=(AnalogTrigger &&rhs) {
m_trigger = std::move(rhs.m_trigger);
std::swap(m_analogInput, rhs.m_analogInput);
std::swap(m_dutyCycle, rhs.m_dutyCycle);
diff --git a/frc971/wpilib/ahal/AnalogTrigger.h b/frc971/wpilib/ahal/AnalogTrigger.h
index 9fad5db..fdc12de 100644
--- a/frc971/wpilib/ahal/AnalogTrigger.h
+++ b/frc971/wpilib/ahal/AnalogTrigger.h
@@ -7,10 +7,10 @@
#pragma once
-#include <memory>
-
#include <hal/Types.h>
+#include <memory>
+
#include "frc971/wpilib/ahal/AnalogTriggerOutput.h"
namespace frc {
@@ -24,13 +24,13 @@
public:
explicit AnalogTrigger(int channel);
- explicit AnalogTrigger(AnalogInput* channel);
- explicit AnalogTrigger(DutyCycle* dutyCycle);
+ explicit AnalogTrigger(AnalogInput *channel);
+ explicit AnalogTrigger(DutyCycle *dutyCycle);
virtual ~AnalogTrigger();
- AnalogTrigger(AnalogTrigger&& rhs);
- AnalogTrigger& operator=(AnalogTrigger&& rhs);
+ AnalogTrigger(AnalogTrigger &&rhs);
+ AnalogTrigger &operator=(AnalogTrigger &&rhs);
void SetLimitsVoltage(double lower, double upper);
@@ -55,8 +55,8 @@
private:
hal::Handle<HAL_AnalogTriggerHandle> m_trigger;
- AnalogInput* m_analogInput = nullptr;
- DutyCycle* m_dutyCycle = nullptr;
+ AnalogInput *m_analogInput = nullptr;
+ DutyCycle *m_dutyCycle = nullptr;
bool m_ownsAnalog = false;
};
diff --git a/frc971/wpilib/ahal/AnalogTriggerOutput.cc b/frc971/wpilib/ahal/AnalogTriggerOutput.cc
index 1bbef1b..cbe40ba 100644
--- a/frc971/wpilib/ahal/AnalogTriggerOutput.cc
+++ b/frc971/wpilib/ahal/AnalogTriggerOutput.cc
@@ -35,7 +35,7 @@
int AnalogTriggerOutput::GetChannel() const { return m_trigger->GetIndex(); }
-AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger& trigger,
+AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger &trigger,
AnalogTriggerType outputType)
: m_trigger(&trigger), m_outputType(outputType) {
HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
diff --git a/frc971/wpilib/ahal/AnalogTriggerOutput.h b/frc971/wpilib/ahal/AnalogTriggerOutput.h
index 8896f37..687d9de 100644
--- a/frc971/wpilib/ahal/AnalogTriggerOutput.h
+++ b/frc971/wpilib/ahal/AnalogTriggerOutput.h
@@ -7,8 +7,8 @@
#pragma once
-#include "hal/AnalogTrigger.h"
#include "frc971/wpilib/ahal/DigitalSource.h"
+#include "hal/AnalogTrigger.h"
namespace frc {
@@ -66,7 +66,7 @@
// Uses pointer rather than smart pointer because a user can not construct
// an AnalogTriggerOutput themselves and because the AnalogTriggerOutput
// should always be in scope at the same time as an AnalogTrigger.
- const AnalogTrigger* m_trigger;
+ const AnalogTrigger *m_trigger;
AnalogTriggerType m_outputType;
};
diff --git a/frc971/wpilib/ahal/Compressor.cc b/frc971/wpilib/ahal/Compressor.cc
index ba04e52..97f8af1 100644
--- a/frc971/wpilib/ahal/Compressor.cc
+++ b/frc971/wpilib/ahal/Compressor.cc
@@ -155,7 +155,8 @@
int32_t status = 0;
bool value;
- value = HAL_GetCTREPCMCompressorCurrentTooHighFault(m_compressorHandle, &status);
+ value =
+ HAL_GetCTREPCMCompressorCurrentTooHighFault(m_compressorHandle, &status);
if (status) {
wpi_setWPIError(Timeout);
@@ -179,8 +180,8 @@
int32_t status = 0;
bool value;
- value =
- HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(m_compressorHandle, &status);
+ value = HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(m_compressorHandle,
+ &status);
if (status) {
wpi_setWPIError(Timeout);
@@ -204,7 +205,8 @@
int32_t status = 0;
bool value;
- value = HAL_GetCTREPCMCompressorShortedStickyFault(m_compressorHandle, &status);
+ value =
+ HAL_GetCTREPCMCompressorShortedStickyFault(m_compressorHandle, &status);
if (status) {
wpi_setWPIError(Timeout);
@@ -247,7 +249,8 @@
int32_t status = 0;
bool value;
- value = HAL_GetCTREPCMCompressorNotConnectedStickyFault(m_compressorHandle, &status);
+ value = HAL_GetCTREPCMCompressorNotConnectedStickyFault(m_compressorHandle,
+ &status);
if (status) {
wpi_setWPIError(Timeout);
@@ -267,7 +270,8 @@
int32_t status = 0;
bool value;
- value = HAL_GetCTREPCMCompressorNotConnectedFault(m_compressorHandle, &status);
+ value =
+ HAL_GetCTREPCMCompressorNotConnectedFault(m_compressorHandle, &status);
if (status) {
wpi_setWPIError(Timeout);
diff --git a/frc971/wpilib/ahal/Compressor.h b/frc971/wpilib/ahal/Compressor.h
index c358ce1..7f46bc5 100644
--- a/frc971/wpilib/ahal/Compressor.h
+++ b/frc971/wpilib/ahal/Compressor.h
@@ -10,8 +10,8 @@
#include <memory>
#include <string>
-#include "hal/Types.h"
#include "frc971/wpilib/ahal/SensorBase.h"
+#include "hal/Types.h"
namespace frc {
diff --git a/frc971/wpilib/ahal/Counter.cc b/frc971/wpilib/ahal/Counter.cc
index 8e91c82..5b4ce97 100644
--- a/frc971/wpilib/ahal/Counter.cc
+++ b/frc971/wpilib/ahal/Counter.cc
@@ -7,11 +7,11 @@
#include "frc971/wpilib/ahal/Counter.h"
-#include "hal/HAL.h"
#include "frc971/wpilib/ahal/AnalogTrigger.h"
#include "frc971/wpilib/ahal/Base.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/WPIErrors.h"
+#include "hal/HAL.h"
using namespace frc;
@@ -31,7 +31,7 @@
ClearDownSource();
}
-Counter::Counter(DigitalSource* source) : Counter(kTwoPulse) {
+Counter::Counter(DigitalSource *source) : Counter(kTwoPulse) {
SetUpSource(source);
ClearDownSource();
}
@@ -41,13 +41,13 @@
ClearDownSource();
}
-Counter::Counter(const AnalogTrigger& trigger) : Counter(kTwoPulse) {
+Counter::Counter(const AnalogTrigger &trigger) : Counter(kTwoPulse) {
SetUpSource(trigger.CreateOutput(AnalogTriggerType::kState));
ClearDownSource();
}
-Counter::Counter(EncodingType encodingType, DigitalSource* upSource,
- DigitalSource* downSource, bool inverted)
+Counter::Counter(EncodingType encodingType, DigitalSource *upSource,
+ DigitalSource *downSource, bool inverted)
: Counter(encodingType,
std::shared_ptr<DigitalSource>(upSource,
NullDeleter<DigitalSource>()),
@@ -96,7 +96,7 @@
SetUpSource(std::make_shared<DigitalInput>(channel));
}
-void Counter::SetUpSource(AnalogTrigger* analogTrigger,
+void Counter::SetUpSource(AnalogTrigger *analogTrigger,
AnalogTriggerType triggerType) {
SetUpSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
NullDeleter<AnalogTrigger>()),
@@ -109,7 +109,7 @@
SetUpSource(analogTrigger->CreateOutput(triggerType));
}
-void Counter::SetUpSource(DigitalSource* source) {
+void Counter::SetUpSource(DigitalSource *source) {
SetUpSource(
std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
}
@@ -125,7 +125,7 @@
HAL_CHECK_STATUS(status);
}
-void Counter::SetUpSource(DigitalSource& source) {
+void Counter::SetUpSource(DigitalSource &source) {
SetUpSource(
std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
}
@@ -157,7 +157,7 @@
SetDownSource(std::make_shared<DigitalInput>(channel));
}
-void Counter::SetDownSource(AnalogTrigger* analogTrigger,
+void Counter::SetDownSource(AnalogTrigger *analogTrigger,
AnalogTriggerType triggerType) {
SetDownSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
NullDeleter<AnalogTrigger>()),
@@ -170,12 +170,12 @@
SetDownSource(analogTrigger->CreateOutput(triggerType));
}
-void Counter::SetDownSource(DigitalSource* source) {
+void Counter::SetDownSource(DigitalSource *source) {
SetDownSource(
std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
}
-void Counter::SetDownSource(DigitalSource& source) {
+void Counter::SetDownSource(DigitalSource &source) {
SetDownSource(
std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
}
diff --git a/frc971/wpilib/ahal/Counter.h b/frc971/wpilib/ahal/Counter.h
index b2c6ecb..1f7c4e0 100644
--- a/frc971/wpilib/ahal/Counter.h
+++ b/frc971/wpilib/ahal/Counter.h
@@ -10,11 +10,11 @@
#include <memory>
#include <string>
-#include "hal/Counter.h"
-#include "hal/Types.h"
#include "frc971/wpilib/ahal/AnalogTrigger.h"
#include "frc971/wpilib/ahal/CounterBase.h"
#include "frc971/wpilib/ahal/SensorBase.h"
+#include "hal/Counter.h"
+#include "hal/Types.h"
namespace frc {
@@ -80,7 +80,7 @@
* @param source A pointer to the existing DigitalSource object. It will be
* set as the Up Source.
*/
- explicit Counter(DigitalSource* source);
+ explicit Counter(DigitalSource *source);
/**
* Create an instance of a counter from a Digital Source (such as a Digital
@@ -107,7 +107,7 @@
*
* @param trigger The reference to the existing AnalogTrigger object.
*/
- explicit Counter(const AnalogTrigger& trigger);
+ explicit Counter(const AnalogTrigger &trigger);
/**
* Create an instance of a Counter object.
@@ -121,8 +121,8 @@
* source
* @param inverted True to invert the output (reverse the direction)
*/
- Counter(EncodingType encodingType, DigitalSource* upSource,
- DigitalSource* downSource, bool inverted);
+ Counter(EncodingType encodingType, DigitalSource *upSource,
+ DigitalSource *downSource, bool inverted);
/**
* Create an instance of a Counter object.
@@ -141,8 +141,8 @@
~Counter() override;
- Counter(Counter&&) = default;
- Counter& operator=(Counter&&) = default;
+ Counter(Counter &&) = default;
+ Counter &operator=(Counter &&) = default;
/**
* Set the upsource for the counter as a digital input channel.
@@ -160,7 +160,7 @@
* @param triggerType The analog trigger output that will trigger the
* counter.
*/
- void SetUpSource(AnalogTrigger* analogTrigger, AnalogTriggerType triggerType);
+ void SetUpSource(AnalogTrigger *analogTrigger, AnalogTriggerType triggerType);
/**
* Set the up counting source to be an analog trigger.
@@ -173,7 +173,7 @@
void SetUpSource(std::shared_ptr<AnalogTrigger> analogTrigger,
AnalogTriggerType triggerType);
- void SetUpSource(DigitalSource* source);
+ void SetUpSource(DigitalSource *source);
/**
* Set the source object that causes the counter to count up.
@@ -191,7 +191,7 @@
*
* @param source Reference to the DigitalSource object to set as the up source
*/
- void SetUpSource(DigitalSource& source);
+ void SetUpSource(DigitalSource &source);
/**
* Set the edge sensitivity on an up counting source.
@@ -224,7 +224,7 @@
* @param triggerType The analog trigger output that will trigger the
* counter.
*/
- void SetDownSource(AnalogTrigger* analogTrigger,
+ void SetDownSource(AnalogTrigger *analogTrigger,
AnalogTriggerType triggerType);
/**
@@ -245,7 +245,7 @@
*
* @param source Pointer to the DigitalSource object to set as the down source
*/
- void SetDownSource(DigitalSource* source);
+ void SetDownSource(DigitalSource *source);
/**
* Set the source object that causes the counter to count down.
@@ -255,7 +255,7 @@
* @param source Reference to the DigitalSource object to set as the down
* source
*/
- void SetDownSource(DigitalSource& source);
+ void SetDownSource(DigitalSource &source);
void SetDownSource(std::shared_ptr<DigitalSource> source);
diff --git a/frc971/wpilib/ahal/DigitalGlitchFilter.cc b/frc971/wpilib/ahal/DigitalGlitchFilter.cc
index 83e6201..2b48fc9 100644
--- a/frc971/wpilib/ahal/DigitalGlitchFilter.cc
+++ b/frc971/wpilib/ahal/DigitalGlitchFilter.cc
@@ -10,10 +10,11 @@
#include <algorithm>
#include <array>
+#include "glog/logging.h"
+
#include "frc971/wpilib/ahal/Counter.h"
#include "frc971/wpilib/ahal/Encoder.h"
#include "frc971/wpilib/ahal/WPIErrors.h"
-#include "glog/logging.h"
#include "hal/Constants.h"
#include "hal/DIO.h"
#include "hal/HAL.h"
@@ -31,7 +32,8 @@
m_channelIndex = std::distance(m_filterAllocated.begin(), index);
*index = true;
- HAL_Report(HALUsageReporting::kResourceType_DigitalGlitchFilter, m_channelIndex);
+ HAL_Report(HALUsageReporting::kResourceType_DigitalGlitchFilter,
+ m_channelIndex);
}
DigitalGlitchFilter::~DigitalGlitchFilter() {
diff --git a/frc971/wpilib/ahal/DigitalGlitchFilter.h b/frc971/wpilib/ahal/DigitalGlitchFilter.h
index d31b9d5..0dd1ffb 100644
--- a/frc971/wpilib/ahal/DigitalGlitchFilter.h
+++ b/frc971/wpilib/ahal/DigitalGlitchFilter.h
@@ -39,7 +39,8 @@
void SetPeriodCycles(int fpga_cycles);
void SetPeriodNanoSeconds(uint64_t nanoseconds);
- // Sets the filter period such that it will work well for an input at a maxmium frequency of hz.
+ // Sets the filter period such that it will work well for an input at a
+ // maxmium frequency of hz.
void SetPeriodHz(int hz);
int GetPeriodCycles();
diff --git a/frc971/wpilib/ahal/DigitalInput.cc b/frc971/wpilib/ahal/DigitalInput.cc
index d922cae..9e97a5d 100644
--- a/frc971/wpilib/ahal/DigitalInput.cc
+++ b/frc971/wpilib/ahal/DigitalInput.cc
@@ -10,10 +10,10 @@
#include <limits>
#include <sstream>
+#include "frc971/wpilib/ahal/WPIErrors.h"
#include "hal/DIO.h"
#include "hal/HAL.h"
#include "hal/Ports.h"
-#include "frc971/wpilib/ahal/WPIErrors.h"
using namespace frc;
diff --git a/frc971/wpilib/ahal/DigitalOutput.cc b/frc971/wpilib/ahal/DigitalOutput.cc
index ac00df3..389cfae 100644
--- a/frc971/wpilib/ahal/DigitalOutput.cc
+++ b/frc971/wpilib/ahal/DigitalOutput.cc
@@ -10,10 +10,10 @@
#include <limits>
#include <sstream>
+#include "frc971/wpilib/ahal/WPIErrors.h"
#include "hal/DIO.h"
#include "hal/HAL.h"
#include "hal/Ports.h"
-#include "frc971/wpilib/ahal/WPIErrors.h"
using namespace frc;
@@ -80,10 +80,10 @@
}
/**
- * Gets the value being output from the Digital Output.
- *
- * @return the state of the digital output.
- */
+ * Gets the value being output from the Digital Output.
+ *
+ * @return the state of the digital output.
+ */
bool DigitalOutput::Get() const {
if (StatusIsFatal()) return false;
diff --git a/frc971/wpilib/ahal/DigitalOutput.h b/frc971/wpilib/ahal/DigitalOutput.h
index c6139ce..4d882c7 100644
--- a/frc971/wpilib/ahal/DigitalOutput.h
+++ b/frc971/wpilib/ahal/DigitalOutput.h
@@ -10,8 +10,8 @@
#include <memory>
#include <string>
-#include "hal/Types.h"
#include "frc971/wpilib/ahal/DigitalSource.h"
+#include "hal/Types.h"
namespace frc {
diff --git a/frc971/wpilib/ahal/DigitalSource.h b/frc971/wpilib/ahal/DigitalSource.h
index f5ece9c..48fa26e 100644
--- a/frc971/wpilib/ahal/DigitalSource.h
+++ b/frc971/wpilib/ahal/DigitalSource.h
@@ -7,8 +7,8 @@
#pragma once
-#include "hal/Types.h"
#include "frc971/wpilib/ahal/InterruptableSensorBase.h"
+#include "hal/Types.h"
namespace frc {
diff --git a/frc971/wpilib/ahal/DriverStation.cc b/frc971/wpilib/ahal/DriverStation.cc
index 8762203..967ebb4 100644
--- a/frc971/wpilib/ahal/DriverStation.cc
+++ b/frc971/wpilib/ahal/DriverStation.cc
@@ -13,11 +13,12 @@
#include <string_view>
#include "FRC_NetworkCommunication/FRCComm.h"
+#include "wpi/SmallString.h"
+
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/WPIErrors.h"
#include "hal/HAL.h"
#include "hal/Power.h"
-#include "wpi/SmallString.h"
using namespace frc;
diff --git a/frc971/wpilib/ahal/DriverStation.h b/frc971/wpilib/ahal/DriverStation.h
index 8bcdcf1..cd520f8 100644
--- a/frc971/wpilib/ahal/DriverStation.h
+++ b/frc971/wpilib/ahal/DriverStation.h
@@ -9,14 +9,14 @@
#include <atomic>
// #include <condition_variable>
-#include <memory>
#include <functional>
+#include <memory>
#include <string>
#include <string_view>
#include <thread>
-#include "hal/DriverStation.h"
#include "frc971/wpilib/ahal/SensorBase.h"
+#include "hal/DriverStation.h"
namespace frc {
@@ -33,8 +33,10 @@
static DriverStation &GetInstance();
static void ReportError(const std::string_view &error);
static void ReportWarning(const std::string_view &error);
- static void ReportError(bool is_error, int code, const std::string_view &error,
- const std::string_view &location, const std::string_view &stack);
+ static void ReportError(bool is_error, int code,
+ const std::string_view &error,
+ const std::string_view &location,
+ const std::string_view &stack);
static const int kJoystickPorts = 6;
diff --git a/frc971/wpilib/ahal/DutyCycle.cc b/frc971/wpilib/ahal/DutyCycle.cc
index b400cf3..f795f78 100644
--- a/frc971/wpilib/ahal/DutyCycle.cc
+++ b/frc971/wpilib/ahal/DutyCycle.cc
@@ -8,8 +8,8 @@
#include "frc971/wpilib/ahal/DutyCycle.h"
#include <hal/DutyCycle.h>
-#include <hal/HAL.h>
#include <hal/FRCUsageReporting.h>
+#include <hal/HAL.h>
#include "frc971/wpilib/ahal/Base.h"
#include "frc971/wpilib/ahal/DigitalSource.h"
@@ -17,7 +17,7 @@
using namespace frc;
-DutyCycle::DutyCycle(DigitalSource* source)
+DutyCycle::DutyCycle(DigitalSource *source)
: m_source{source, NullDeleter<DigitalSource>()} {
if (m_source == nullptr) {
wpi_setWPIError(NullParameter);
@@ -26,7 +26,7 @@
}
}
-DutyCycle::DutyCycle(DigitalSource& source)
+DutyCycle::DutyCycle(DigitalSource &source)
: m_source{&source, NullDeleter<DigitalSource>()} {
InitDutyCycle();
}
diff --git a/frc971/wpilib/ahal/DutyCycle.h b/frc971/wpilib/ahal/DutyCycle.h
index 88d6a54..3f99d94 100644
--- a/frc971/wpilib/ahal/DutyCycle.h
+++ b/frc971/wpilib/ahal/DutyCycle.h
@@ -7,10 +7,10 @@
#pragma once
-#include <memory>
-
#include <hal/Types.h>
+#include <memory>
+
namespace frc {
class DigitalSource;
class AnalogTrigger;
@@ -41,7 +41,7 @@
*
* @param source The DigitalSource to use.
*/
- explicit DutyCycle(DigitalSource& source);
+ explicit DutyCycle(DigitalSource &source);
/**
* Constructs a DutyCycle input from a DigitalSource input.
*
@@ -49,7 +49,7 @@
*
* @param source The DigitalSource to use.
*/
- explicit DutyCycle(DigitalSource* source);
+ explicit DutyCycle(DigitalSource *source);
/**
* Constructs a DutyCycle input from a DigitalSource input.
*
@@ -64,8 +64,8 @@
*/
virtual ~DutyCycle();
- DutyCycle(DutyCycle&&) = default;
- DutyCycle& operator=(DutyCycle&&) = default;
+ DutyCycle(DutyCycle &&) = default;
+ DutyCycle &operator=(DutyCycle &&) = default;
/**
* Get the frequency of the duty cycle signal.
diff --git a/frc971/wpilib/ahal/Encoder.cc b/frc971/wpilib/ahal/Encoder.cc
index 417792d..76a7163 100644
--- a/frc971/wpilib/ahal/Encoder.cc
+++ b/frc971/wpilib/ahal/Encoder.cc
@@ -7,9 +7,9 @@
#include "frc971/wpilib/ahal/Encoder.h"
-#include "hal/HAL.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/WPIErrors.h"
+#include "hal/HAL.h"
using namespace frc;
diff --git a/frc971/wpilib/ahal/ErrorBase.h b/frc971/wpilib/ahal/ErrorBase.h
index f0dfe3e..ef1dd5d 100644
--- a/frc971/wpilib/ahal/ErrorBase.h
+++ b/frc971/wpilib/ahal/ErrorBase.h
@@ -12,42 +12,42 @@
#include <wpi/mutex.h>
// Forward declared manually to avoid needing to pull in entire HAL header.
-extern "C" const char* HAL_GetErrorMessage(int32_t code);
+extern "C" const char *HAL_GetErrorMessage(int32_t code);
#define wpi_setErrnoErrorWithContext(context) \
this->SetErrnoError((context), __FILE__, __FUNCTION__, __LINE__)
#define wpi_setErrnoError() wpi_setErrnoErrorWithContext("")
-#define wpi_setImaqErrorWithContext(code, context) \
- do { \
+#define wpi_setImaqErrorWithContext(code, context) \
+ do { \
} while (0)
-#define wpi_setErrorWithContext(code, context) \
- do { \
+#define wpi_setErrorWithContext(code, context) \
+ do { \
} while (0)
-#define wpi_setErrorWithContextRange(code, min, max, req, context) \
- do { \
+#define wpi_setErrorWithContextRange(code, min, max, req, context) \
+ do { \
} while (0)
-#define wpi_setHALError(code) \
- do { \
+#define wpi_setHALError(code) \
+ do { \
} while (0)
-#define wpi_setHALErrorWithRange(code, min, max, req) \
- do { \
+#define wpi_setHALErrorWithRange(code, min, max, req) \
+ do { \
} while (0)
#define wpi_setError(code) wpi_setErrorWithContext(code, "")
-#define wpi_setStaticErrorWithContext(object, code, context) \
- do { \
+#define wpi_setStaticErrorWithContext(object, code, context) \
+ do { \
} while (0)
#define wpi_setStaticError(object, code) \
wpi_setStaticErrorWithContext(object, code, "")
-#define wpi_setGlobalErrorWithContext(code, context) \
- do { \
+#define wpi_setGlobalErrorWithContext(code, context) \
+ do { \
} while (0)
-#define wpi_setGlobalHALError(code) \
- do { \
+#define wpi_setGlobalHALError(code) \
+ do { \
} while (0)
#define wpi_setGlobalError(code) wpi_setGlobalErrorWithContext(code, "")
diff --git a/frc971/wpilib/ahal/InterruptableSensorBase.cc b/frc971/wpilib/ahal/InterruptableSensorBase.cc
index 59eb962..06ee513 100644
--- a/frc971/wpilib/ahal/InterruptableSensorBase.cc
+++ b/frc971/wpilib/ahal/InterruptableSensorBase.cc
@@ -7,8 +7,9 @@
#include "frc971/wpilib/ahal/InterruptableSensorBase.h"
-#include "frc971/wpilib/ahal/WPIErrors.h"
#include "glog/logging.h"
+
+#include "frc971/wpilib/ahal/WPIErrors.h"
#include "hal/HAL.h"
using namespace frc;
diff --git a/frc971/wpilib/ahal/PWM.cc b/frc971/wpilib/ahal/PWM.cc
index 7184643..3c42bc4 100644
--- a/frc971/wpilib/ahal/PWM.cc
+++ b/frc971/wpilib/ahal/PWM.cc
@@ -6,12 +6,13 @@
/*----------------------------------------------------------------------------*/
#include "hal/PWM.h"
-#include "frc971/wpilib/ahal/PWM.h"
#include <sstream>
-#include "frc971/wpilib/ahal/WPIErrors.h"
#include "glog/logging.h"
+
+#include "frc971/wpilib/ahal/PWM.h"
+#include "frc971/wpilib/ahal/WPIErrors.h"
#include "hal/HAL.h"
#include "hal/Ports.h"
diff --git a/frc971/wpilib/ahal/PWM.h b/frc971/wpilib/ahal/PWM.h
index c6e7a25..5c13d35 100644
--- a/frc971/wpilib/ahal/PWM.h
+++ b/frc971/wpilib/ahal/PWM.h
@@ -10,8 +10,8 @@
#include <memory>
#include <string>
-#include "hal/Types.h"
#include "frc971/wpilib/ahal/SensorBase.h"
+#include "hal/Types.h"
namespace frc {
diff --git a/frc971/wpilib/ahal/PowerDistributionPanel.cc b/frc971/wpilib/ahal/PowerDistributionPanel.cc
index 39087fb..e9487e7 100644
--- a/frc971/wpilib/ahal/PowerDistributionPanel.cc
+++ b/frc971/wpilib/ahal/PowerDistributionPanel.cc
@@ -9,10 +9,10 @@
#include <sstream>
-#include "hal/HAL.h"
-#include "hal/PowerDistribution.h"
-#include "hal/Ports.h"
#include "frc971/wpilib/ahal/WPIErrors.h"
+#include "hal/HAL.h"
+#include "hal/Ports.h"
+#include "hal/PowerDistribution.h"
using namespace frc;
#define WPI_LIB_FATAL_ERROR(tag, msg)
diff --git a/frc971/wpilib/ahal/PowerDistributionPanel.h b/frc971/wpilib/ahal/PowerDistributionPanel.h
index dc80158..7471a78 100644
--- a/frc971/wpilib/ahal/PowerDistributionPanel.h
+++ b/frc971/wpilib/ahal/PowerDistributionPanel.h
@@ -7,11 +7,11 @@
#pragma once
+#include <hal/Types.h>
+
#include <memory>
#include <string>
-#include <hal/Types.h>
-
#include "frc971/wpilib/ahal/SensorBase.h"
namespace frc {
diff --git a/frc971/wpilib/ahal/Relay.cc b/frc971/wpilib/ahal/Relay.cc
index 51241be..13aa13a 100644
--- a/frc971/wpilib/ahal/Relay.cc
+++ b/frc971/wpilib/ahal/Relay.cc
@@ -6,13 +6,13 @@
/*----------------------------------------------------------------------------*/
#include "hal/Relay.h"
-#include "frc971/wpilib/ahal/Relay.h"
#include <sstream>
+#include "frc971/wpilib/ahal/Relay.h"
+#include "frc971/wpilib/ahal/WPIErrors.h"
#include "hal/HAL.h"
#include "hal/Ports.h"
-#include "frc971/wpilib/ahal/WPIErrors.h"
using namespace frc;
@@ -51,7 +51,8 @@
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
int32_t status = 0;
- m_reverseHandle = HAL_InitializeRelayPort(portHandle, false, nullptr, &status);
+ m_reverseHandle =
+ HAL_InitializeRelayPort(portHandle, false, nullptr, &status);
if (status != 0) {
wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
channel, HAL_GetErrorMessage(status));
diff --git a/frc971/wpilib/ahal/Relay.h b/frc971/wpilib/ahal/Relay.h
index 389947c..0eb10ca 100644
--- a/frc971/wpilib/ahal/Relay.h
+++ b/frc971/wpilib/ahal/Relay.h
@@ -10,8 +10,8 @@
#include <memory>
#include <string>
-#include "hal/Types.h"
#include "frc971/wpilib/ahal/SensorBase.h"
+#include "hal/Types.h"
namespace frc {
diff --git a/frc971/wpilib/ahal/RobotBase.cc b/frc971/wpilib/ahal/RobotBase.cc
index 3cc39df..3640191 100644
--- a/frc971/wpilib/ahal/RobotBase.cc
+++ b/frc971/wpilib/ahal/RobotBase.cc
@@ -9,9 +9,9 @@
#include <cstdio>
-#include "hal/HAL.h"
#include "frc971/wpilib/ahal/DriverStation.h"
#include "frc971/wpilib/ahal/WPILibVersion.h"
+#include "hal/HAL.h"
using namespace frc;
diff --git a/frc971/wpilib/ahal/RobotBase.h b/frc971/wpilib/ahal/RobotBase.h
index 5b4ce5b..4cb3d8a 100644
--- a/frc971/wpilib/ahal/RobotBase.h
+++ b/frc971/wpilib/ahal/RobotBase.h
@@ -12,8 +12,8 @@
#include <thread>
#include "aos/realtime.h"
-#include "hal/HAL.h"
#include "frc971/wpilib/ahal/Base.h"
+#include "hal/HAL.h"
namespace frc {
diff --git a/frc971/wpilib/ahal/SPI.cc b/frc971/wpilib/ahal/SPI.cc
index 8568210..874858b 100644
--- a/frc971/wpilib/ahal/SPI.cc
+++ b/frc971/wpilib/ahal/SPI.cc
@@ -8,13 +8,14 @@
#include "frc971/wpilib/ahal/SPI.h"
#include <hal/SPI.h>
-#include <wpi/SmallVector.h>
-#include <wpi/mutex.h>
#include <cstring>
#include <utility>
#include "absl/types/span.h"
+#include <wpi/SmallVector.h>
+#include <wpi/mutex.h>
+
#include "frc971/wpilib/ahal/DigitalSource.h"
#include "frc971/wpilib/ahal/WPIErrors.h"
diff --git a/frc971/wpilib/ahal/SPI.h b/frc971/wpilib/ahal/SPI.h
index 542db80..6151ded 100644
--- a/frc971/wpilib/ahal/SPI.h
+++ b/frc971/wpilib/ahal/SPI.h
@@ -8,13 +8,13 @@
#pragma once
#include <hal/SPITypes.h>
-#include <wpi/deprecated.h>
#include <cstdint>
#include <memory>
#include <span>
#include "absl/types/span.h"
+#include <wpi/deprecated.h>
namespace frc {
diff --git a/frc971/wpilib/ahal/SensorBase.cc b/frc971/wpilib/ahal/SensorBase.cc
index 111e4c6..4716187 100644
--- a/frc971/wpilib/ahal/SensorBase.cc
+++ b/frc971/wpilib/ahal/SensorBase.cc
@@ -8,6 +8,7 @@
#include "frc971/wpilib/ahal/SensorBase.h"
#include "FRC_NetworkCommunication/LoadOut.h"
+
#include "frc971/wpilib/ahal/WPIErrors.h"
#include "hal/AnalogInput.h"
#include "hal/AnalogOutput.h"
diff --git a/frc971/wpilib/ahal/TalonFX.cc b/frc971/wpilib/ahal/TalonFX.cc
index fe29089..ccd30db 100644
--- a/frc971/wpilib/ahal/TalonFX.cc
+++ b/frc971/wpilib/ahal/TalonFX.cc
@@ -39,4 +39,3 @@
HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel());
}
-
diff --git a/frc971/wpilib/buffered_pcm.cc b/frc971/wpilib/buffered_pcm.cc
index 8f5a3d9..f356722 100644
--- a/frc971/wpilib/buffered_pcm.cc
+++ b/frc971/wpilib/buffered_pcm.cc
@@ -3,9 +3,9 @@
#include <cinttypes>
#include "aos/logging/logging.h"
+#include "hal/CTREPCM.h"
#include "hal/HAL.h"
#include "hal/Ports.h"
-#include "hal/CTREPCM.h"
namespace frc971 {
namespace wpilib {
diff --git a/frc971/wpilib/buffered_pcm.h b/frc971/wpilib/buffered_pcm.h
index 033ded6..f743371 100644
--- a/frc971/wpilib/buffered_pcm.h
+++ b/frc971/wpilib/buffered_pcm.h
@@ -1,10 +1,10 @@
#ifndef FRC971_WPILIB_BUFFERED_PCM_H_
#define FRC971_WPILIB_BUFFERED_PCM_H_
-#include <memory>
-
#include <hal/HAL.h>
+#include <memory>
+
#include "frc971/wpilib/buffered_solenoid.h"
namespace frc971 {
diff --git a/frc971/wpilib/buffered_solenoid.cc b/frc971/wpilib/buffered_solenoid.cc
index c07a7cd..a4b337e 100644
--- a/frc971/wpilib/buffered_solenoid.cc
+++ b/frc971/wpilib/buffered_solenoid.cc
@@ -5,9 +5,7 @@
namespace frc971 {
namespace wpilib {
-void BufferedSolenoid::Set(bool value) {
- pcm_->DoSet(number_, value);
-}
+void BufferedSolenoid::Set(bool value) { pcm_->DoSet(number_, value); }
} // namespace wpilib
} // namespace frc971
diff --git a/frc971/wpilib/dma.cc b/frc971/wpilib/dma.cc
index a2b2d20..cff1ac1 100644
--- a/frc971/wpilib/dma.cc
+++ b/frc971/wpilib/dma.cc
@@ -4,10 +4,11 @@
#include <cstring>
#include <type_traits>
+#include "glog/logging.h"
+
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/DigitalSource.h"
#include "frc971/wpilib/ahal/Encoder.h"
-#include "glog/logging.h"
#include "hal/HAL.h"
// Interface to the roboRIO FPGA's DMA features.
diff --git a/frc971/wpilib/drivetrain_writer.h b/frc971/wpilib/drivetrain_writer.h
index c27638f..195b5a3 100644
--- a/frc971/wpilib/drivetrain_writer.h
+++ b/frc971/wpilib/drivetrain_writer.h
@@ -2,7 +2,6 @@
#define FRC971_WPILIB_DRIVETRAIN_WRITER_H_
#include "aos/commonmath.h"
-
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/wpilib/ahal/PWM.h"
#include "frc971/wpilib/loop_output_handler.h"
diff --git a/frc971/wpilib/encoder_and_potentiometer.h b/frc971/wpilib/encoder_and_potentiometer.h
index 5089620..7ee85e1 100644
--- a/frc971/wpilib/encoder_and_potentiometer.h
+++ b/frc971/wpilib/encoder_and_potentiometer.h
@@ -7,12 +7,10 @@
#include "aos/macros.h"
#include "aos/time/time.h"
-
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/Counter.h"
#include "frc971/wpilib/ahal/DigitalSource.h"
#include "frc971/wpilib/ahal/Encoder.h"
-
#include "frc971/wpilib/dma.h"
#include "frc971/wpilib/dma_edge_counting.h"
diff --git a/frc971/wpilib/fpga_time_conversion.h b/frc971/wpilib/fpga_time_conversion.h
index 9862918..51cc7a9 100644
--- a/frc971/wpilib/fpga_time_conversion.h
+++ b/frc971/wpilib/fpga_time_conversion.h
@@ -1,11 +1,12 @@
#ifndef FRC971_WPILIB_FPGA_TIME_CONVERSION_H_
#define FRC971_WPILIB_FPGA_TIME_CONVERSION_H_
-#include <optional>
#include <chrono>
+#include <optional>
+
+#include "glog/logging.h"
#include "aos/time/time.h"
-#include "glog/logging.h"
#include "hal/cpp/fpga_clock.h"
namespace frc971 {
diff --git a/frc971/wpilib/interrupt_edge_counting.h b/frc971/wpilib/interrupt_edge_counting.h
index dd48f2d..5ea8df5 100644
--- a/frc971/wpilib/interrupt_edge_counting.h
+++ b/frc971/wpilib/interrupt_edge_counting.h
@@ -9,7 +9,6 @@
#include "aos/logging/logging.h"
#include "aos/macros.h"
#include "aos/stl_mutex/stl_mutex.h"
-
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/Encoder.h"
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index bb99dc8..f8bfb5a 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -128,7 +128,8 @@
last_monotonic_now_ = monotonic_now;
monotonic_clock::time_point last_tick_timepoint = GetPWMStartTime();
- VLOG(1) << "Start time " << last_tick_timepoint << " period " << period_.count();
+ VLOG(1) << "Start time " << last_tick_timepoint << " period "
+ << period_.count();
if (last_tick_timepoint == monotonic_clock::min_time) {
return;
}
@@ -137,7 +138,8 @@
((monotonic_now - chrono::microseconds(FLAGS_pwm_offset) -
last_tick_timepoint) /
period_) *
- period_ + chrono::microseconds(FLAGS_pwm_offset);
+ period_ +
+ chrono::microseconds(FLAGS_pwm_offset);
VLOG(1) << "Now " << monotonic_now << " tick " << last_tick_timepoint;
// If it's over 1/2 of a period back in time, that's wrong. Move it
// forwards to now.
@@ -149,8 +151,7 @@
// after the falling edge. This gives us a little bit of buffer for
// errors in waking up. The PWM cycle starts at the falling edge of the
// PWM pulse.
- const auto next_time =
- last_tick_timepoint + period_;
+ const auto next_time = last_tick_timepoint + period_;
timer_handler_->Setup(next_time, period_);
}
diff --git a/frc971/zeroing/absolute_and_absolute_encoder_test.cc b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
index 59b421a..b28f637 100644
--- a/frc971/zeroing/absolute_and_absolute_encoder_test.cc
+++ b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
@@ -1,10 +1,11 @@
#include "frc971/zeroing/absolute_and_absolute_encoder.h"
-#include "frc971/zeroing/zeroing_test.h"
#include "glog/logging.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
+#include "frc971/zeroing/zeroing_test.h"
+
namespace frc971 {
namespace zeroing {
namespace testing {
diff --git a/frc971/zeroing/absolute_encoder.cc b/frc971/zeroing/absolute_encoder.cc
index d0cd0d9..aa68acf 100644
--- a/frc971/zeroing/absolute_encoder.cc
+++ b/frc971/zeroing/absolute_encoder.cc
@@ -3,9 +3,10 @@
#include <cmath>
#include <numeric>
+#include "glog/logging.h"
+
#include "aos/containers/error_list.h"
#include "frc971/zeroing/wrap.h"
-#include "glog/logging.h"
namespace frc971 {
namespace zeroing {
diff --git a/frc971/zeroing/absolute_encoder_test.cc b/frc971/zeroing/absolute_encoder_test.cc
index ce485eb..fdc1fa0 100644
--- a/frc971/zeroing/absolute_encoder_test.cc
+++ b/frc971/zeroing/absolute_encoder_test.cc
@@ -1,9 +1,10 @@
#include "frc971/zeroing/absolute_encoder.h"
-#include "frc971/zeroing/zeroing_test.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
+#include "frc971/zeroing/zeroing_test.h"
+
namespace frc971 {
namespace zeroing {
namespace testing {
diff --git a/frc971/zeroing/averager_test.cc b/frc971/zeroing/averager_test.cc
index 9f8f727..50eac72 100644
--- a/frc971/zeroing/averager_test.cc
+++ b/frc971/zeroing/averager_test.cc
@@ -1,6 +1,7 @@
-#include "gtest/gtest.h"
#include "frc971/zeroing/averager.h"
+#include "gtest/gtest.h"
+
namespace frc971 {
namespace zeroing {
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index 0872b1c..c06ed43 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -80,7 +80,7 @@
last_gyro_sample_ << values.gyro_x(), values.gyro_y(), values.gyro_z();
gyro_averager_.AddData(last_gyro_sample_);
last_accel_sample_ << values.accelerometer_x(), values.accelerometer_y(),
- values.accelerometer_z();
+ values.accelerometer_z();
accel_averager_.AddData(last_accel_sample_);
return true;
}
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index 7fad58c..2a5036d 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -48,14 +48,14 @@
// Max variation (difference between the maximum and minimum value) in a
// kSamplesToAverage range before we allow using the samples for zeroing.
// These values are currently based on looking at results from the ADIS16448.
- static constexpr double kGyroMaxVariation = 0.02; // rad / sec
+ static constexpr double kGyroMaxVariation = 0.02; // rad / sec
// Maximum magnitude we allow the gyro zero to have--this is used to prevent
// us from zeroing the gyro if we just happen to be spinning at a very
// consistent non-zero rate. Currently this is only plausible in simulation.
static constexpr double kGyroMaxZeroingMagnitude = 0.1; // rad / sec
// Max variation in the range before we consider the accelerometer readings to
// be steady.
- static constexpr double kAccelMaxVariation = 0.05; // g's
+ static constexpr double kAccelMaxVariation = 0.05; // g's
// If we ever are able to rezero and get a zero that is more than
// kGyroFaultVariation away from the original zeroing, fault.
static constexpr double kGyroFaultVariation = 0.05; // rad / sec
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index deff1b6..566a7b8 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -1,7 +1,9 @@
-#include "aos/flatbuffers.h"
-#include "gtest/gtest.h"
#include "frc971/zeroing/imu_zeroer.h"
+#include "gtest/gtest.h"
+
+#include "aos/flatbuffers.h"
+
namespace frc971::zeroing {
static constexpr int kMinSamplesToZero =
@@ -113,7 +115,8 @@
(static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 0.001;
zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + offset},
- {4 + offset, 5 + offset, 6 + offset}).message());
+ {4 + offset, 5 + offset, 6 + offset})
+ .message());
}
ASSERT_TRUE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
@@ -143,7 +146,8 @@
(static_cast<double>(ii) / (kMinSamplesToZero - 1) - 0.5) * 1.0;
zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.01 + offset, 0.02 + offset, 0.03 + offset},
- {4 + offset, 5 + offset, 6 + offset}).message());
+ {4 + offset, 5 + offset, 6 + offset})
+ .message());
}
ASSERT_FALSE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
diff --git a/frc971/zeroing/pot_and_absolute_encoder.cc b/frc971/zeroing/pot_and_absolute_encoder.cc
index 32c4f60..f62b393 100644
--- a/frc971/zeroing/pot_and_absolute_encoder.cc
+++ b/frc971/zeroing/pot_and_absolute_encoder.cc
@@ -3,9 +3,10 @@
#include <cmath>
#include <numeric>
+#include "glog/logging.h"
+
#include "aos/containers/error_list.h"
#include "frc971/zeroing/wrap.h"
-#include "glog/logging.h"
namespace frc971 {
namespace zeroing {
diff --git a/frc971/zeroing/pot_and_absolute_encoder.h b/frc971/zeroing/pot_and_absolute_encoder.h
index 2ff141f..d250a50 100644
--- a/frc971/zeroing/pot_and_absolute_encoder.h
+++ b/frc971/zeroing/pot_and_absolute_encoder.h
@@ -3,8 +3,9 @@
#include <vector>
-#include "aos/containers/error_list.h"
#include "flatbuffers/flatbuffers.h"
+
+#include "aos/containers/error_list.h"
#include "frc971/zeroing/zeroing.h"
namespace frc971 {
diff --git a/frc971/zeroing/pot_and_absolute_encoder_test.cc b/frc971/zeroing/pot_and_absolute_encoder_test.cc
index 1784fed..5c05cc1 100644
--- a/frc971/zeroing/pot_and_absolute_encoder_test.cc
+++ b/frc971/zeroing/pot_and_absolute_encoder_test.cc
@@ -1,9 +1,10 @@
#include "frc971/zeroing/pot_and_absolute_encoder.h"
-#include "frc971/zeroing/zeroing_test.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
+#include "frc971/zeroing/zeroing_test.h"
+
namespace frc971 {
namespace zeroing {
namespace testing {
diff --git a/frc971/zeroing/pot_and_index.cc b/frc971/zeroing/pot_and_index.cc
index 7c713d9..0f1e9e0 100644
--- a/frc971/zeroing/pot_and_index.cc
+++ b/frc971/zeroing/pot_and_index.cc
@@ -78,8 +78,8 @@
// If there are no index pulses to use or we don't have enough samples yet to
// have a well-filtered starting position then we use the filtered value as
// our best guess.
- if (!zeroed_ &&
- (info.index_pulses() == last_used_index_pulse_count_ || !offset_ready())) {
+ if (!zeroed_ && (info.index_pulses() == last_used_index_pulse_count_ ||
+ !offset_ready())) {
offset_ = start_average;
} else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses()) {
// Note the accurate start position and the current index pulse count so
diff --git a/frc971/zeroing/relative_encoder_test.cc b/frc971/zeroing/relative_encoder_test.cc
index a29ba4b..6256fe7 100644
--- a/frc971/zeroing/relative_encoder_test.cc
+++ b/frc971/zeroing/relative_encoder_test.cc
@@ -1,6 +1,7 @@
+#include "gtest/gtest.h"
+
#include "frc971/zeroing/zeroing.h"
#include "frc971/zeroing/zeroing_test.h"
-#include "gtest/gtest.h"
namespace frc971 {
namespace zeroing {
diff --git a/frc971/zeroing/unwrap_test.cc b/frc971/zeroing/unwrap_test.cc
index d1e6db9..b71c8b6 100644
--- a/frc971/zeroing/unwrap_test.cc
+++ b/frc971/zeroing/unwrap_test.cc
@@ -1,6 +1,7 @@
-#include "frc971/zeroing/wrap.h"
#include "gtest/gtest.h"
+#include "frc971/zeroing/wrap.h"
+
namespace frc971 {
namespace zeroing {
namespace testing {
diff --git a/frc971/zeroing/wrap_test.cc b/frc971/zeroing/wrap_test.cc
index b5f9100..6597e03 100644
--- a/frc971/zeroing/wrap_test.cc
+++ b/frc971/zeroing/wrap_test.cc
@@ -34,8 +34,8 @@
break;
}
}
- EXPECT_TRUE(found_interval) << ": Wrap(" << i << ", " << j
- << ") = " << wrapped_val;
+ EXPECT_TRUE(found_interval)
+ << ": Wrap(" << i << ", " << j << ") = " << wrapped_val;
}
}
}
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 028194c..f7b52a6 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -7,6 +7,7 @@
#include <vector>
#include "flatbuffers/flatbuffers.h"
+
#include "frc971/constants.h"
#include "frc971/control_loops/control_loops_generated.h"
diff --git a/frc971/zeroing/zeroing_test.h b/frc971/zeroing/zeroing_test.h
index 9b7ba95..cadbe6c 100644
--- a/frc971/zeroing/zeroing_test.h
+++ b/frc971/zeroing/zeroing_test.h
@@ -1,8 +1,8 @@
-#include "frc971/zeroing/zeroing.h"
+#include "gtest/gtest.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/position_sensor_sim.h"
-#include "gtest/gtest.h"
+#include "frc971/zeroing/zeroing.h"
namespace frc971 {
namespace zeroing {