Move aos/controls to frc971/control_loops
Also put what was aos/controls/control_loops.fbs in y2012/control_loops
because that's the only user.
Change-Id: I8f402b0708103077e135a41e55ef5e4f23681d87
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index ec480c0..f44abe0 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -315,8 +315,8 @@
":localizer",
":spline_goal_fbs",
"//aos:math",
- "//aos/controls:control_loop",
- "//aos/controls:polytope",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:polytope",
"//frc971/input:robot_state_fbs",
"//aos/util:log_interval",
"//aos/util:trapezoid_profile",
@@ -344,7 +344,7 @@
":drivetrain_states",
":gear",
"//aos:math",
- "//aos/controls:polytope",
+ "//frc971/control_loops:polytope",
"//frc971/control_loops:coerce_goal",
"//frc971/control_loops:control_loops_fbs",
":spline_goal_fbs",
@@ -425,7 +425,7 @@
":spline_goal_fbs",
":splinedrivetrain",
":ssdrivetrain",
- "//aos/controls:control_loop",
+ "//frc971/control_loops:control_loop",
"//aos/util:log_interval",
"//frc971/control_loops:runge_kutta",
"//frc971/queues:gyro_fbs",
@@ -497,7 +497,7 @@
":drivetrain_position_fbs",
":drivetrain_output_fbs",
":drivetrain_test_lib",
- "//aos/controls:control_loop_test",
+ "//frc971/control_loops:control_loop_test",
"//aos/events/logging:log_writer",
"//aos/testing:googletest",
"//frc971/queues:gyro_fbs",
@@ -728,7 +728,7 @@
deps = [
":drivetrain_config",
":drivetrain_status_fbs",
- "//aos/controls:quaternion_utils",
+ "//frc971/control_loops:quaternion_utils",
"//aos/events:event_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:runge_kutta",
@@ -746,7 +746,7 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
":drivetrain_test_lib",
- "//aos/controls:quaternion_utils",
+ "//frc971/control_loops:quaternion_utils",
"//aos/testing:googletest",
"//aos/testing:random_seed",
"//frc971/control_loops/drivetrain:improved_down_estimator",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 91f1513..5242550 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -2,10 +2,11 @@
#include <sched.h>
#include <stdio.h>
+
#include <cmath>
#include <memory>
-#include "Eigen/Dense"
+#include "Eigen/Dense"
#include "aos/logging/logging.h"
#include "frc971/control_loops/drivetrain/down_estimator.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -47,7 +48,7 @@
right_high_requested_(dt_config_.default_high_gear) {
last_voltage_.setZero();
last_last_voltage_.setZero();
- aos::controls::HPolytope<0>::Init();
+ frc971::controls::HPolytope<0>::Init();
event_loop->OnRun([this]() {
// On the first fetch, make sure that we are caught all the way up to the
// present.
@@ -300,8 +301,8 @@
::aos::EventLoop *event_loop,
LocalizerInterface *localizer,
const ::std::string &name)
- : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
- name),
+ : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
dt_config_(dt_config),
filters_(dt_config, event_loop, localizer),
dt_openloop_(dt_config_, filters_.kf()),
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 325715f..efaf53d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -2,9 +2,8 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
#include "Eigen/Dense"
-
-#include "aos/controls/control_loop.h"
-#include "aos/controls/polytope.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/polytope.h"
#include "aos/util/log_interval.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -142,7 +141,7 @@
};
class DrivetrainLoop
- : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
+ : public frc971::controls::ControlLoop<Goal, Position, Status, Output> {
public:
// Note that we only actually store N - 1 splines, since we need to keep one
// fetcher free to check whether there are any new splines.
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 64fb0b8..749b998 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -3,16 +3,11 @@
#include <chrono>
#include <memory>
-#include "aos/controls/control_loop_test.h"
-#include "aos/controls/polytope.h"
#include "aos/events/event_loop.h"
#include "aos/events/logging/log_writer.h"
#include "aos/time/time.h"
-#include "gflags/gflags.h"
-#include "gtest/gtest.h"
-#include "gmock/gmock.h"
-
#include "frc971/control_loops/coerce_goal.h"
+#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
@@ -20,9 +15,13 @@
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/drivetrain/trajectory_generator.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#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.");
@@ -35,10 +34,10 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
-class DrivetrainTest : public ::aos::testing::ControlLoopTest {
+class DrivetrainTest : public ::frc971::testing::ControlLoopTest {
protected:
DrivetrainTest()
- : ::aos::testing::ControlLoopTest(
+ : ::frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig(
"frc971/control_loops/drivetrain/simulation_config.json"),
GetTestDrivetrainConfig().dt),
@@ -581,7 +580,6 @@
VerifyNearSplineGoal();
}
-
// Tests that we can drive a spline backwards.
TEST_F(DrivetrainTest, SplineSimpleBackwards) {
SetEnabled(true);
@@ -741,7 +739,6 @@
EXPECT_FALSE(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
->has_y());
}
-
}
// Tests that a spline can't be restarted.
@@ -869,8 +866,8 @@
}
INSTANTIATE_TEST_SUITE_P(DriveSplinesForwardsAndBackwards,
- DrivetrainBackwardsParamTest,
- ::testing::Values(false, true));
+ DrivetrainBackwardsParamTest,
+ ::testing::Values(false, true));
// Tests that simple spline converges when it starts to the side of where it
// thinks.
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index 56e7536..2c129f7 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -2,8 +2,7 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
-
-#include "aos/controls/quaternion_utils.h"
+#include "frc971/control_loops/quaternion_utils.h"
namespace frc971 {
namespace control_loops {
@@ -80,7 +79,7 @@
// We now have the sigma points after the model update.
// Compute the mean of the transformed sigma point
- X_hat_ = Eigen::Quaternion<double>(aos::controls::QuaternionMean(Y));
+ X_hat_ = Eigen::Quaternion<double>(frc971::controls::QuaternionMean(Y));
// And the covariance.
Eigen::Matrix<double, 3, 2 * 3 + 1> Wprime;
@@ -159,7 +158,7 @@
// Update X_hat and the covariance P
X_hat_ = X_hat_ * Eigen::Quaternion<double>(
- aos::controls::ToQuaternionFromRotationVector(
+ frc971::controls::ToQuaternionFromRotationVector(
K * (measurement - Z_hat_)));
P_ = P_prior - K * P_vv * K.transpose();
}
@@ -215,7 +214,7 @@
for (int i = 0; i < 7; ++i) {
// Compute the error vector for each sigma point.
Eigen::Matrix<double, 3, 1> Wprimei =
- aos::controls::ToRotationVectorFromQuaternion(
+ frc971::controls::ToRotationVectorFromQuaternion(
Eigen::Quaternion<double>(mean).conjugate() *
Eigen::Quaternion<double>(points.col(i)));
// Now, compute the contribution of this sigma point to P_prior.
@@ -241,7 +240,7 @@
Eigen::Matrix<double, 4, 3 * 2 + 1> X;
for (int i = 0; i < 3; ++i) {
Eigen::Quaternion<double> perturbation(
- aos::controls::ToQuaternionFromRotationVector(S.col(i), M_PI_2));
+ frc971::controls::ToQuaternionFromRotationVector(S.col(i), M_PI_2));
X.col(i * 2) = (mean * perturbation).coeffs();
X.col(i * 2 + 1) = (mean * perturbation.conjugate()).coeffs();
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 9894f68..8aed6b2 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -3,7 +3,7 @@
#include <Eigen/Geometry>
#include <random>
-#include "aos/controls/quaternion_utils.h"
+#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"
@@ -235,7 +235,7 @@
drivetrain::GenerateSigmaPoints(mean, covariance);
const Eigen::Matrix<double, 4, 1> calculated_mean =
- aos::controls::QuaternionMean(vectors);
+ frc971::controls::QuaternionMean(vectors);
VLOG(1) << "actual mean: " << mean.coeffs();
VLOG(1) << "calculated mean: " << calculated_mean;
@@ -266,7 +266,7 @@
drivetrain::GenerateSigmaPoints(mean, covariance);
const Eigen::Matrix<double, 4, 1> calculated_mean =
- aos::controls::QuaternionMean(vectors);
+ frc971::controls::QuaternionMean(vectors);
Eigen::Matrix<double, 3, 3 * 2 + 1> Wprime;
Eigen::Matrix<double, 3, 3> calculated_covariance =
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index fe733bb..aea030e 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -2,7 +2,7 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_H_
#include "aos/commonmath.h"
-#include "aos/controls/polytope.h"
+#include "frc971/control_loops/polytope.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/gear.h"
#ifdef __linux__
@@ -80,7 +80,7 @@
StateFeedbackLoop<7, 2, 4, Scalar> *kf_;
- const ::aos::controls::HVPolytope<2, 4, 4, Scalar> U_Poly_;
+ const ::frc971::controls::HVPolytope<2, 4, 4, Scalar> U_Poly_;
::std::unique_ptr<StateFeedbackLoop<2, 2, 2, Scalar>> loop_;
@@ -340,12 +340,12 @@
const Scalar equality_w = kZero;
// Construct a constraint on R by manipulating the constraint on U
- ::aos::controls::HVPolytope<2, 4, 4, Scalar> R_poly_hv(
+ ::frc971::controls::HVPolytope<2, 4, 4, Scalar> R_poly_hv(
U_Poly_.static_H() * (loop_->controller().K() + FF),
U_Poly_.static_k() +
U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
(loop_->controller().K() + FF).inverse() *
- ::aos::controls::ShiftPoints<2, 4, Scalar>(
+ ::frc971::controls::ShiftPoints<2, 4, Scalar>(
U_Poly_.StaticVertices(),
loop_->controller().K() * loop_->X_hat()));
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 3e58d09..594175c 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -1,8 +1,7 @@
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "aos/commonmath.h"
-#include "aos/controls/polytope.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"
@@ -33,10 +32,10 @@
(Eigen::Matrix<double, 2, 4>() << /*[[*/ 1.0, 1.0, -1.0, -1.0 /*]*/,
/*[*/ -1.0, 1.0, 1.0, -1.0 /*]*/)
.finished()),
- linear_profile_(::aos::controls::kLoopFrequency),
- angular_profile_(::aos::controls::kLoopFrequency),
+ linear_profile_(::frc971::controls::kLoopFrequency),
+ angular_profile_(::frc971::controls::kLoopFrequency),
localizer_(localizer) {
- ::aos::controls::HPolytope<0>::Init();
+ ::frc971::controls::HPolytope<0>::Init();
T_ << 1, 1, 1, -1;
T_inverse_ = T_.inverse();
unprofiled_goal_.setZero();
@@ -87,13 +86,13 @@
error_K * Eigen::Matrix<double, 2, 1>(kf_->X_hat(kLeftError),
kf_->X_hat(kRightError));
- const ::aos::controls::HVPolytope<2, 4, 4> pos_poly_hv(
+ const ::frc971::controls::HVPolytope<2, 4, 4> pos_poly_hv(
U_poly_.static_H() * position_K * T_,
U_poly_.static_H() *
(-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
(U_poly_.static_k() * max_voltage_),
(position_K * T_).inverse() *
- ::aos::controls::ShiftPoints<2, 4, double>(
+ ::frc971::controls::ShiftPoints<2, 4, double>(
(U_poly_.StaticVertices() * max_voltage_),
-velocity_K * velocity_error + U_integral - kf_->ff_U()));
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index d83473d..be42abf 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 "aos/controls/control_loop.h"
-#include "aos/controls/polytope.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_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -56,7 +55,7 @@
// Reprsents +/- full power on each motor in U-space, aka the square from
// (-12, -12) to (12, 12).
- const ::aos::controls::HVPolytope<2, 4, 4> U_poly_;
+ const ::frc971::controls::HVPolytope<2, 4, 4> U_poly_;
// multiplying by T converts [left_error, right_error] to
// [left_right_error_difference, total_distance_error].