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/y2014/control_loops/claw/BUILD b/y2014/control_loops/claw/BUILD
index f8865a5..a79dc78 100644
--- a/y2014/control_loops/claw/BUILD
+++ b/y2014/control_loops/claw/BUILD
@@ -75,8 +75,8 @@
":claw_position_fbs",
":claw_status_fbs",
"//aos:math",
- "//aos/controls:control_loop",
- "//aos/controls:polytope",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:polytope",
"//frc971/control_loops:coerce_goal",
"//frc971/control_loops:hall_effect_tracker",
"//frc971/control_loops:state_feedback_loop",
@@ -97,7 +97,7 @@
":claw_output_fbs",
":claw_position_fbs",
":claw_status_fbs",
- "//aos/controls:control_loop_test",
+ "//frc971/control_loops:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
"//frc971/control_loops:team_number_test_environment",
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 56ca6b0..4f0248c 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -2,9 +2,8 @@
#include <algorithm>
-#include "aos/logging/logging.h"
#include "aos/commonmath.h"
-
+#include "aos/logging/logging.h"
#include "y2014/constants.h"
#include "y2014/control_loops/claw/claw_motor_plant.h"
@@ -46,8 +45,8 @@
namespace claw {
using ::frc971::HallEffectTracker;
-using ::y2014::control_loops::claw::kDt;
using ::frc971::control_loops::DoCoerceGoal;
+using ::y2014::control_loops::claw::kDt;
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
@@ -57,20 +56,18 @@
: StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
uncapped_average_voltage_(0.0),
is_zeroing_(true),
- U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
- -1, 0,
- 0, 1,
- 0, -1).finished(),
+ U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
+ .finished(),
(Eigen::Matrix<double, 4, 1>() << kMaxVoltage, kMaxVoltage,
- kMaxVoltage, kMaxVoltage).finished()),
- U_Poly_zeroing_((Eigen::Matrix<double, 4, 2>() << 1, 0,
- -1, 0,
- 0, 1,
- 0, -1).finished(),
- (Eigen::Matrix<double, 4, 1>() <<
- kZeroingVoltage, kZeroingVoltage,
- kZeroingVoltage, kZeroingVoltage).finished()) {
- ::aos::controls::HPolytope<0>::Init();
+ kMaxVoltage, kMaxVoltage)
+ .finished()),
+ U_Poly_zeroing_(
+ (Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << kZeroingVoltage, kZeroingVoltage,
+ kZeroingVoltage, kZeroingVoltage)
+ .finished()) {
+ ::frc971::controls::HPolytope<0>::Init();
}
// Caps the voltage prioritizing reducing velocity error over reducing
@@ -119,8 +116,8 @@
const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
const Eigen::Matrix<double, 4, 1> pos_poly_k =
poly.k() - poly.H() * velocity_K * velocity_error;
- const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
- const ::aos::controls::HVPolytope<2, 4, 4> hv_pos_poly(
+ const ::frc971::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+ const ::frc971::controls::HVPolytope<2, 4, 4> hv_pos_poly(
pos_poly_H, pos_poly_k, pos_poly.Vertices());
Eigen::Matrix<double, 2, 1> adjusted_pos_error;
@@ -187,7 +184,8 @@
{
const auto values = constants::GetValues().claw;
if (top_known_) {
- if (X_hat(0, 0) + X_hat(1, 0) > values.upper_claw.upper_limit && U(1, 0) > 0) {
+ if (X_hat(0, 0) + X_hat(1, 0) > values.upper_claw.upper_limit &&
+ U(1, 0) > 0) {
AOS_LOG(WARNING, "upper claw too high and moving up\n");
mutable_U(1, 0) = 0;
} else if (X_hat(0, 0) + X_hat(1, 0) < values.upper_claw.lower_limit &&
@@ -337,7 +335,6 @@
}
}
-
void BottomZeroedStateFeedbackLoop::HandleCalibrationError(
const constants::Values::Claws::Claw &claw_values) {
double edge_encoder;
@@ -369,8 +366,8 @@
}
ClawMotor::ClawMotor(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
- name),
+ : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -692,9 +689,9 @@
bottom_claw_.HandleCalibrationError(values.claw.lower_claw);
top_claw_.HandleCalibrationError(values.claw.upper_claw);
} else if (top_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
bottom_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
// Time to fine tune the zero.
// Limit the goals here.
if (!enabled) {
@@ -920,8 +917,8 @@
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
- R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
- claw_.R(3, 0);
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
+ claw_.R(2, 0), claw_.R(3, 0);
claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
AOS_LOG(DEBUG,
@@ -943,8 +940,8 @@
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
- R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
- claw_.R(3, 0);
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
+ claw_.R(2, 0), claw_.R(3, 0);
claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
AOS_LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
@@ -954,16 +951,16 @@
if (output) {
if (goal) {
- //setup the intake
+ // setup the intake
output_struct.intake_voltage =
(goal->intake() > 12.0)
? 12
: (goal->intake() < -12.0) ? -12.0 : goal->intake();
output_struct.tusk_voltage = goal->centering();
output_struct.tusk_voltage =
- (goal->centering() > 12.0) ? 12 : (goal->centering() < -12.0)
- ? -12.0
- : goal->centering();
+ (goal->centering() > 12.0)
+ ? 12
+ : (goal->centering() < -12.0) ? -12.0 : goal->centering();
}
output_struct.top_claw_voltage = claw_.U(1, 0);
output_struct.bottom_claw_voltage = claw_.U(0, 0);
diff --git a/y2014/control_loops/claw/claw.h b/y2014/control_loops/claw/claw.h
index 5bcccc1..07a5d2a 100644
--- a/y2014/control_loops/claw/claw.h
+++ b/y2014/control_loops/claw/claw.h
@@ -3,8 +3,8 @@
#include <memory>
-#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 "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/hall_effect_tracker.h"
#include "frc971/control_loops/state_feedback_loop.h"
@@ -49,7 +49,7 @@
bool top_known_ = false, bottom_known_ = false;
- const ::aos::controls::HPolytope<2> U_Poly_, U_Poly_zeroing_;
+ const ::frc971::controls::HPolytope<2> U_Poly_, U_Poly_zeroing_;
};
class ClawMotor;
@@ -88,7 +88,9 @@
double absolute_position() const { return encoder() + offset(); }
const ::frc971::HallEffectTracker &front() const { return front_; }
- const ::frc971::HallEffectTracker &calibration() const { return calibration_; }
+ const ::frc971::HallEffectTracker &calibration() const {
+ return calibration_;
+ }
const ::frc971::HallEffectTracker &back() const { return back_; }
bool any_hall_effect_changed() const {
@@ -143,8 +145,8 @@
double last_off_encoder_;
bool any_triggered_last_;
- const ::frc971::HallEffectTracker* posedge_filter_ = nullptr;
- const ::frc971::HallEffectTracker* negedge_filter_ = nullptr;
+ const ::frc971::HallEffectTracker *posedge_filter_ = nullptr;
+ const ::frc971::HallEffectTracker *negedge_filter_ = nullptr;
private:
// Does the edges of 1 sensor for GetPositionOfEdge.
@@ -187,7 +189,7 @@
};
class ClawMotor
- : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
+ : public frc971::controls::ControlLoop<Goal, Position, Status, Output> {
public:
explicit ClawMotor(::aos::EventLoop *event_loop,
const ::std::string &name = "/claw");
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index 5222ad8..eb941a1 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -3,7 +3,7 @@
#include <chrono>
#include <memory>
-#include "aos/controls/control_loop_test.h"
+#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2014/constants.h"
@@ -19,15 +19,15 @@
namespace claw {
namespace testing {
-using ::frc971::HallEffectStructT;
using ::aos::monotonic_clock;
+using ::frc971::HallEffectStructT;
namespace chrono = ::std::chrono;
typedef enum {
- TOP_CLAW = 0,
- BOTTOM_CLAW,
+ TOP_CLAW = 0,
+ BOTTOM_CLAW,
- CLAW_COUNT
+ CLAW_COUNT
} ClawType;
// Class which simulates the wrist and sends out queue messages containing the
@@ -62,7 +62,8 @@
AOS_LOG(INFO, "Reinitializing to {top: %f, bottom: %f}\n",
initial_top_position, initial_bottom_position);
claw_plant_->mutable_X(0, 0) = initial_bottom_position;
- claw_plant_->mutable_X(1, 0) = initial_top_position - initial_bottom_position;
+ claw_plant_->mutable_X(1, 0) =
+ initial_top_position - initial_bottom_position;
claw_plant_->mutable_X(2, 0) = 0.0;
claw_plant_->mutable_X(3, 0) = 0.0;
claw_plant_->mutable_Y() = claw_plant_->C() * claw_plant_->X();
@@ -199,7 +200,8 @@
UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
initial_position, half_claw->front.get(),
- *last_position.front.get(), claw.front, claw_name, "front");
+ *last_position.front.get(), claw.front, claw_name,
+ "front");
UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
initial_position, half_claw->calibration.get(),
@@ -245,7 +247,7 @@
// Simulates the claw moving for one timestep.
void Simulate() {
- const constants::Values& v = constants::GetValues();
+ const constants::Values &v = constants::GetValues();
EXPECT_TRUE(claw_output_fetcher_.Fetch());
Eigen::Matrix<double, 2, 1> U;
@@ -286,10 +288,10 @@
PositionT last_position_;
};
-class ClawTest : public ::aos::testing::ControlLoopTest {
+class ClawTest : public ::frc971::testing::ControlLoopTest {
protected:
ClawTest()
- : ::aos::testing::ControlLoopTest(
+ : ::frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig("y2014/config.json"),
chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop("test")),
@@ -431,10 +433,9 @@
EXPECT_NEAR(7.5, top_goal, 1e-4);
}
-
class ZeroingClawTest
: public ClawTest,
- public ::testing::WithParamInterface< ::std::pair<double, double>> {};
+ public ::testing::WithParamInterface<::std::pair<double, double>> {};
// Tests that the wrist zeros correctly starting on the hall effect sensor.
TEST_P(ZeroingClawTest, ParameterizedZero) {
@@ -454,29 +455,19 @@
VerifyNearGoal();
}
-INSTANTIATE_TEST_SUITE_P(ZeroingClawTest, ZeroingClawTest,
- ::testing::Values(::std::make_pair(0.04, 0.02),
- ::std::make_pair(0.2, 0.1),
- ::std::make_pair(0.3, 0.2),
- ::std::make_pair(0.4, 0.3),
- ::std::make_pair(0.5, 0.4),
- ::std::make_pair(0.6, 0.5),
- ::std::make_pair(0.7, 0.6),
- ::std::make_pair(0.8, 0.7),
- ::std::make_pair(0.9, 0.8),
- ::std::make_pair(1.0, 0.9),
- ::std::make_pair(1.1, 1.0),
- ::std::make_pair(1.15, 1.05),
- ::std::make_pair(1.05, 0.95),
- ::std::make_pair(1.2, 1.1),
- ::std::make_pair(1.3, 1.2),
- ::std::make_pair(1.4, 1.3),
- ::std::make_pair(1.5, 1.4),
- ::std::make_pair(1.6, 1.5),
- ::std::make_pair(1.7, 1.6),
- ::std::make_pair(1.8, 1.7),
- ::std::make_pair(2.015, 2.01)
-));
+INSTANTIATE_TEST_SUITE_P(
+ ZeroingClawTest, ZeroingClawTest,
+ ::testing::Values(::std::make_pair(0.04, 0.02), ::std::make_pair(0.2, 0.1),
+ ::std::make_pair(0.3, 0.2), ::std::make_pair(0.4, 0.3),
+ ::std::make_pair(0.5, 0.4), ::std::make_pair(0.6, 0.5),
+ ::std::make_pair(0.7, 0.6), ::std::make_pair(0.8, 0.7),
+ ::std::make_pair(0.9, 0.8), ::std::make_pair(1.0, 0.9),
+ ::std::make_pair(1.1, 1.0), ::std::make_pair(1.15, 1.05),
+ ::std::make_pair(1.05, 0.95), ::std::make_pair(1.2, 1.1),
+ ::std::make_pair(1.3, 1.2), ::std::make_pair(1.4, 1.3),
+ ::std::make_pair(1.5, 1.4), ::std::make_pair(1.6, 1.5),
+ ::std::make_pair(1.7, 1.6), ::std::make_pair(1.8, 1.7),
+ ::std::make_pair(2.015, 2.01)));
/*
// Tests that loosing the encoder for a second triggers a re-zero.
@@ -559,7 +550,7 @@
monotonic_clock::time_point start_time, double offset) {
SetEnabled(true);
int capped_count = 0;
- const constants::Values& values = constants::GetValues();
+ const constants::Values &values = constants::GetValues();
bool kicked = false;
bool measured = false;
while (test_event_loop_->monotonic_now() <