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/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 =