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/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;
}