Convert control loop tests over to simulated event loop
This makes it so that we properly only use ShmEventLoop for running in
realtime on a robot. Very nice.
Change-Id: I46b770b336f59e08cfaf28511b3bd5689f72fff1
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 773b6cb..3d6c930 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -243,6 +243,17 @@
],
)
+queue_library(
+ name = "static_zeroing_single_dof_profiled_subsystem_test_queue",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test.q",
+ ],
+ deps = [
+ ":profiled_subsystem_queue",
+ ":queues",
+ ],
+)
+
cc_library(
name = "profiled_subsystem",
srcs = [
@@ -407,6 +418,7 @@
":position_sensor_sim",
":static_zeroing_single_dof_profiled_subsystem",
":static_zeroing_single_dof_profiled_subsystem_test_plants",
+ ":static_zeroing_single_dof_profiled_subsystem_test_queue",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
],
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b45f016..b9d2bc4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -93,7 +93,8 @@
const ::frc971::control_loops::DrivetrainQueue::Position *position,
::frc971::control_loops::DrivetrainQueue::Output *output,
::frc971::control_loops::DrivetrainQueue::Status *status) {
- monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ const monotonic_clock::time_point monotonic_now =
+ event_loop()->monotonic_now();
if (!has_been_enabled_ && output) {
has_been_enabled_ = true;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 3f20554..e4de9bb 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -10,9 +10,6 @@
#include "aos/time/time.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
-#if defined(SUPPORT_PLOT)
-#include "third_party/matplotlib-cpp/matplotlibcpp.h"
-#endif
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
@@ -22,8 +19,6 @@
#include "frc971/control_loops/drivetrain/localizer.q.h"
#include "frc971/queues/gyro.q.h"
-DEFINE_bool(plot, false, "If true, plot");
-
namespace frc971 {
namespace control_loops {
namespace drivetrain {
@@ -34,92 +29,59 @@
class DrivetrainTest : public ::aos::testing::ControlLoopTest {
protected:
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory that
- // is no longer valid.
- ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
-
- ::aos::ShmEventLoop event_loop_;
- const DrivetrainConfig<double> dt_config_;
- DeadReckonEkf localizer_;
- // Create a loop and simulation plant.
- DrivetrainLoop drivetrain_motor_;
- ::aos::ShmEventLoop simulation_event_loop_;
- DrivetrainSimulation drivetrain_motor_plant_;
-
DrivetrainTest()
- : my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
- ".frc971.control_loops.drivetrain_queue.goal",
- ".frc971.control_loops.drivetrain_queue.position",
- ".frc971.control_loops.drivetrain_queue.output",
- ".frc971.control_loops.drivetrain_queue.status"),
+ : ::aos::testing::ControlLoopTest(GetTestDrivetrainConfig().dt),
+ test_event_loop_(MakeEventLoop()),
+ drivetrain_goal_sender_(
+ test_event_loop_
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")),
+ drivetrain_goal_fetcher_(
+ test_event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")),
+ drivetrain_status_fetcher_(
+ test_event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
+ drivetrain_output_fetcher_(
+ test_event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".frc971.control_loops.drivetrain_queue.output")),
+ localizer_control_sender_(
+ test_event_loop_->MakeSender<LocalizerControl>(
+ ".frc971.control_loops.drivetrain.localizer_control")),
+ drivetrain_event_loop_(MakeEventLoop()),
dt_config_(GetTestDrivetrainConfig()),
- localizer_(dt_config_),
- drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
- drivetrain_motor_plant_(&simulation_event_loop_, dt_config_) {
+ localizer_(drivetrain_event_loop_.get(), dt_config_),
+ drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
+ drivetrain_plant_event_loop_(MakeEventLoop()),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_) {
set_battery_voltage(12.0);
}
+ virtual ~DrivetrainTest() {}
- void RunIteration() {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true, dt_config_.dt);
- if (FLAGS_plot) {
- my_drivetrain_queue_.status.FetchLatest();
-
- ::Eigen::Matrix<double, 2, 1> actual_position =
- drivetrain_motor_plant_.GetPosition();
- actual_x_.push_back(actual_position(0));
- actual_y_.push_back(actual_position(1));
-
- trajectory_x_.push_back(my_drivetrain_queue_.status->trajectory_logging.x);
- trajectory_y_.push_back(my_drivetrain_queue_.status->trajectory_logging.y);
- }
- }
-
- void RunForTime(monotonic_clock::duration run_for) {
- const auto end_time = monotonic_clock::now() + run_for;
- while (monotonic_clock::now() < end_time) {
- RunIteration();
- }
- }
-
- void TearDown() {
-#if defined(SUPPORT_PLOT)
- if (FLAGS_plot) {
- std::cout << "plotting.\n";
- matplotlibcpp::figure();
- matplotlibcpp::plot(actual_x_, actual_y_, {{"label", "actual position"}});
- matplotlibcpp::plot(trajectory_x_, trajectory_y_,
- {{"label", "trajectory position"}});
- matplotlibcpp::legend();
- matplotlibcpp::show();
- }
-#endif
- }
+ void TearDown() { drivetrain_plant_.MaybePlot(); }
void VerifyNearGoal() {
- my_drivetrain_queue_.goal.FetchLatest();
- my_drivetrain_queue_.position.FetchLatest();
- EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
- drivetrain_motor_plant_.GetLeftPosition(), 1e-3);
- EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
- drivetrain_motor_plant_.GetRightPosition(), 1e-3);
+ drivetrain_goal_fetcher_.Fetch();
+ EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal,
+ drivetrain_plant_.GetLeftPosition(), 1e-3);
+ EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal,
+ drivetrain_plant_.GetRightPosition(), 1e-3);
}
void VerifyNearPosition(double x, double y) {
- my_drivetrain_queue_.status.FetchLatest();
- auto actual = drivetrain_motor_plant_.GetPosition();
+ auto actual = drivetrain_plant_.GetPosition();
EXPECT_NEAR(actual(0), x, 1e-2);
EXPECT_NEAR(actual(1), y, 1e-2);
}
void VerifyNearSplineGoal() {
- my_drivetrain_queue_.status.FetchLatest();
- double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
- double expected_y = my_drivetrain_queue_.status->trajectory_logging.y;
- auto actual = drivetrain_motor_plant_.GetPosition();
+ drivetrain_status_fetcher_.Fetch();
+ const double expected_x = drivetrain_status_fetcher_->trajectory_logging.x;
+ const double expected_y = drivetrain_status_fetcher_->trajectory_logging.y;
+ const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
EXPECT_NEAR(actual(0), expected_x, 2e-2);
EXPECT_NEAR(actual(1), expected_y, 2e-2);
}
@@ -128,104 +90,117 @@
do {
// Run for fewer iterations while the worker thread computes.
::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
- RunIteration();
- my_drivetrain_queue_.status.FetchLatest();
- } while (my_drivetrain_queue_.status->trajectory_logging.planning_state !=
- (int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
+ RunFor(dt());
+ EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+ } while (drivetrain_status_fetcher_->trajectory_logging.planning_state !=
+ (int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
}
void WaitForTrajectoryExecution() {
do {
- RunIteration();
- my_drivetrain_queue_.status.FetchLatest();
- } while (!my_drivetrain_queue_.status->trajectory_logging.is_executed);
+ RunFor(dt());
+ EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+ } while (!drivetrain_status_fetcher_->trajectory_logging.is_executed);
}
- virtual ~DrivetrainTest() {}
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ drivetrain_output_fetcher_;
+ ::aos::Sender<LocalizerControl> localizer_control_sender_;
- private:
- ::std::vector<double> actual_x_;
- ::std::vector<double> actual_y_;
- ::std::vector<double> trajectory_x_;
- ::std::vector<double> trajectory_y_;
+ ::std::unique_ptr<::aos::EventLoop> drivetrain_event_loop_;
+ const DrivetrainConfig<double> dt_config_;
+ DeadReckonEkf localizer_;
+ // Create a loop and simulation plant.
+ DrivetrainLoop drivetrain_;
+ ::std::unique_ptr<::aos::EventLoop> drivetrain_plant_event_loop_;
+ DrivetrainSimulation drivetrain_plant_;
};
// Tests that the drivetrain converges on a goal.
TEST_F(DrivetrainTest, ConvergesCorrectly) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- RunForTime(chrono::seconds(2));
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(2));
VerifyNearGoal();
}
// Tests that the drivetrain converges on a goal when under the effect of a
// voltage offset/disturbance.
TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
- drivetrain_motor_plant_.set_left_voltage_offset(1.0);
- drivetrain_motor_plant_.set_right_voltage_offset(1.0);
- RunForTime(chrono::milliseconds(1500));
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
+ drivetrain_plant_.set_left_voltage_offset(1.0);
+ drivetrain_plant_.set_right_voltage_offset(1.0);
+ RunFor(chrono::milliseconds(1500));
VerifyNearGoal();
}
// Tests that it survives disabling.
TEST_F(DrivetrainTest, SurvivesDisabling) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(-1.0)
- .right_goal(1.0)
- .Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ EXPECT_TRUE(message.Send());
+ }
for (int i = 0; i < 500; ++i) {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
if (i > 20 && i < 200) {
- SimulateTimestep(false, dt_config_.dt);
+ SetEnabled(false);
} else {
- SimulateTimestep(true, dt_config_.dt);
+ SetEnabled(true);
}
+ RunFor(dt());
}
VerifyNearGoal();
}
-// Tests that never having a goal doesn't break.
-TEST_F(DrivetrainTest, NoGoalStart) {
- for (int i = 0; i < 20; ++i) {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
- }
-}
-
// Tests that never having a goal, but having driver's station messages, doesn't
// break.
TEST_F(DrivetrainTest, NoGoalWithRobotState) {
- RunForTime(chrono::milliseconds(100));
+ SetEnabled(true);
+ RunFor(chrono::milliseconds(100));
}
// Tests that the robot successfully drives straight forward.
// This used to not work due to a U-capping bug.
TEST_F(DrivetrainTest, DriveStraightForward) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(4.0)
- .right_goal(4.0)
- .Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 4.0;
+ message->right_goal = 4.0;
+ EXPECT_TRUE(message.Send());
+ }
+
for (int i = 0; i < 500; ++i) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
- my_drivetrain_queue_.output->right_voltage, 1e-4);
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
+ drivetrain_output_fetcher_->right_voltage, 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
}
VerifyNearGoal();
}
@@ -233,16 +208,19 @@
// Tests that the robot successfully drives close to straight.
// This used to fail in simulation due to libcdd issues with U-capping.
TEST_F(DrivetrainTest, DriveAlmostStraightForward) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(1)
- .left_goal(4.0)
- .right_goal(3.9)
- .Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 4.0;
+ message->right_goal = 3.9;
+ EXPECT_TRUE(message.Send());
+ }
for (int i = 0; i < 500; ++i) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -11);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -11);
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
}
VerifyNearGoal();
}
@@ -274,81 +252,78 @@
// Tests that a linear motion profile succeeds.
TEST_F(DrivetrainTest, ProfileStraightForward) {
+ SetEnabled(true);
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 1;
- goal->left_goal = 4.0;
- goal->right_goal = 4.0;
- goal->linear.max_velocity = 1.0;
- goal->linear.max_acceleration = 3.0;
- goal->angular.max_velocity = 1.0;
- goal->angular.max_acceleration = 3.0;
- goal.Send();
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 4.0;
+ message->right_goal = 4.0;
+ message->linear.max_velocity = 1.0;
+ message->linear.max_acceleration = 3.0;
+ message->angular.max_velocity = 1.0;
+ message->angular.max_acceleration = 3.0;
+ EXPECT_TRUE(message.Send());
}
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(6))) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
- my_drivetrain_queue_.output->right_voltage, 1e-4);
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
- EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
- EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+ while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
+ drivetrain_output_fetcher_->right_voltage, 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
}
VerifyNearGoal();
}
// Tests that an angular motion profile succeeds.
TEST_F(DrivetrainTest, ProfileTurn) {
+ SetEnabled(true);
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 1;
- goal->left_goal = -1.0;
- goal->right_goal = 1.0;
- goal->linear.max_velocity = 1.0;
- goal->linear.max_acceleration = 3.0;
- goal->angular.max_velocity = 1.0;
- goal->angular.max_acceleration = 3.0;
- goal.Send();
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = -1.0;
+ message->right_goal = 1.0;
+ message->linear.max_velocity = 1.0;
+ message->linear.max_acceleration = 3.0;
+ message->angular.max_velocity = 1.0;
+ message->angular.max_acceleration = 3.0;
+ EXPECT_TRUE(message.Send());
}
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(6))) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
- -my_drivetrain_queue_.output->right_voltage, 1e-4);
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
- EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
- EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+ while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
+ -drivetrain_output_fetcher_->right_voltage, 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
}
VerifyNearGoal();
}
// Tests that a mixed turn drive saturated profile succeeds.
TEST_F(DrivetrainTest, SaturatedTurnDrive) {
+ SetEnabled(true);
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 1;
- goal->left_goal = 5.0;
- goal->right_goal = 4.0;
- goal->linear.max_velocity = 6.0;
- goal->linear.max_acceleration = 4.0;
- goal->angular.max_velocity = 2.0;
- goal->angular.max_acceleration = 4.0;
- goal.Send();
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 5.0;
+ message->right_goal = 4.0;
+ message->linear.max_velocity = 6.0;
+ message->linear.max_acceleration = 4.0;
+ message->angular.max_velocity = 2.0;
+ message->angular.max_acceleration = 4.0;
+ EXPECT_TRUE(message.Send());
}
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(3))) {
- RunIteration();
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(3))) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
}
VerifyNearGoal();
}
@@ -356,111 +331,121 @@
// Tests that being in teleop drive for a bit and then transitioning to closed
// drive profiles nicely.
TEST_F(DrivetrainTest, OpenLoopThenClosed) {
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(0)
- .wheel(0.0)
- .throttle(1.0)
- .highgear(true)
- .quickturn(false)
- .Send();
-
- RunForTime(chrono::seconds(1));
-
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(0)
- .wheel(0.0)
- .throttle(-0.3)
- .highgear(true)
- .quickturn(false)
- .Send();
-
- RunForTime(chrono::seconds(1));
-
- my_drivetrain_queue_.goal.MakeWithBuilder()
- .controller_type(0)
- .wheel(0.0)
- .throttle(0.0)
- .highgear(true)
- .quickturn(false)
- .Send();
-
- RunForTime(chrono::seconds(10));
-
+ SetEnabled(true);
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- goal = my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 1;
- goal->left_goal = 5.0;
- goal->right_goal = 4.0;
- goal->linear.max_velocity = 1.0;
- goal->linear.max_acceleration = 2.0;
- goal->angular.max_velocity = 1.0;
- goal->angular.max_acceleration = 2.0;
- goal.Send();
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 0;
+ message->wheel = 0.0;
+ message->throttle = 1.0;
+ message->highgear = true;
+ message->quickturn = false;
+ EXPECT_TRUE(message.Send());
}
- const auto end_time = monotonic_clock::now() + chrono::seconds(4);
- while (monotonic_clock::now() < end_time) {
- drivetrain_motor_plant_.SendPositionMessage();
- drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true, dt_config_.dt);
- ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
- EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
- EXPECT_LT(my_drivetrain_queue_.output->left_voltage, 6);
- EXPECT_LT(my_drivetrain_queue_.output->right_voltage, 6);
+ RunFor(chrono::seconds(1));
+
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 0;
+ message->wheel = 0.0;
+ message->throttle = -0.3;
+ message->highgear = true;
+ message->quickturn = false;
+ EXPECT_TRUE(message.Send());
+ }
+
+ RunFor(chrono::seconds(1));
+
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 0;
+ message->wheel = 0.0;
+ message->throttle = 0.0;
+ message->highgear = true;
+ message->quickturn = false;
+ EXPECT_TRUE(message.Send());
+ }
+
+ RunFor(chrono::seconds(10));
+
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 1;
+ message->left_goal = 5.0;
+ message->right_goal = 4.0;
+ message->linear.max_velocity = 1.0;
+ message->linear.max_acceleration = 2.0;
+ message->angular.max_velocity = 1.0;
+ message->angular.max_acceleration = 2.0;
+ EXPECT_TRUE(message.Send());
+ }
+
+ const auto end_time = monotonic_now() + chrono::seconds(4);
+ while (monotonic_now() < end_time) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
}
VerifyNearGoal();
}
// Tests that simple spline converges on a goal.
TEST_F(DrivetrainTest, SplineSimple) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ // Send the start goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that we can drive a spline backwards.
TEST_F(DrivetrainTest, SplineSimpleBackwards) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.drive_spline_backwards = true;
- goal->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
- goal->spline.spline_y = {{0.0, 0.0, -0.25 ,-0.75, -1.0, -1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.drive_spline_backwards = true;
+ message->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
+ message->spline.spline_y = {{0.0, 0.0, -0.25, -0.75, -1.0, -1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
// Check that we are right on the spline at the start (otherwise the feedback
// will tend to correct for going the wrong direction).
for (int ii = 0; ii < 10; ++ii) {
- RunForTime(chrono::milliseconds(100));
+ RunFor(chrono::milliseconds(100));
VerifyNearSplineGoal();
}
@@ -468,249 +453,282 @@
VerifyNearSplineGoal();
// Check that we are pointed the right direction:
- my_drivetrain_queue_.status.FetchLatest();
- auto actual = drivetrain_motor_plant_.state();
+ drivetrain_status_fetcher_.Fetch();
+ auto actual = drivetrain_plant_.state();
const double expected_theta =
- my_drivetrain_queue_.status->trajectory_logging.theta;
+ drivetrain_status_fetcher_->trajectory_logging.theta;
// As a sanity check, compare both against absolute angle and the spline's
// goal angle.
EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
- EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta),
- 2e-2);
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta), 2e-2);
}
// Tests that simple spline with a single goal message.
TEST_F(DrivetrainTest, SplineSingleGoal) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal->spline_handle = 1;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that a trajectory can be stopped in the middle.
TEST_F(DrivetrainTest, SplineStop) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal->spline_handle = 1;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(500));
- my_drivetrain_queue_.status.FetchLatest();
- const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
- const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+ RunFor(chrono::milliseconds(500));
+ drivetrain_status_fetcher_.Fetch();
+ const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
+ const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
- my_drivetrain_queue_.goal.MakeMessage();
- stop_goal->controller_type = 2;
- stop_goal->spline_handle = 0;
- stop_goal.Send();
- RunForTime(chrono::milliseconds(2000));
+ // Now stop.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(2000));
// The goal shouldn't change after being stopped.
- my_drivetrain_queue_.status.FetchLatest();
- EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
- EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
+ drivetrain_status_fetcher_.Fetch();
+ EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
+ EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
}
// Tests that a spline can't be restarted.
TEST_F(DrivetrainTest, SplineRestart) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal->spline_handle = 1;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(500));
- my_drivetrain_queue_.status.FetchLatest();
- const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
- const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+ RunFor(chrono::milliseconds(500));
+ drivetrain_status_fetcher_.Fetch();
+ const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
+ const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
- my_drivetrain_queue_.goal.MakeMessage();
- stop_goal->controller_type = 2;
- stop_goal->spline_handle = 0;
- stop_goal.Send();
- RunForTime(chrono::milliseconds(500));
+ // Send a stop goal.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(500));
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> restart_goal =
- my_drivetrain_queue_.goal.MakeMessage();
- restart_goal->controller_type = 2;
- restart_goal->spline_handle = 1;
- restart_goal.Send();
- RunForTime(chrono::milliseconds(2000));
+ // Send a restart.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(2000));
// The goal shouldn't change after being stopped and restarted.
- my_drivetrain_queue_.status.FetchLatest();
- EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
- EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
+ drivetrain_status_fetcher_.Fetch();
+ EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
+ EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
}
// Tests that simple spline converges when it doesn't start where it thinks.
TEST_F(DrivetrainTest, SplineOffset) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.2, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.2, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that simple spline converges when it starts to the side of where it
// thinks.
TEST_F(DrivetrainTest, SplineSideOffset) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.5, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.5, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that a multispline converges on a goal.
TEST_F(DrivetrainTest, MultiSpline) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 2;
- goal->spline.spline_x = {
- {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- goal->spline.spline_y = {
- {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 2;
+ message->spline.spline_x = {
+ {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {
+ {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(4000));
+ RunFor(chrono::milliseconds(4000));
VerifyNearSplineGoal();
}
// Tests that several splines converges on a goal.
TEST_F(DrivetrainTest, SequentialSplines) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- goal.Send();
- RunIteration();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
WaitForTrajectoryExecution();
VerifyNearSplineGoal();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_spline_goal->controller_type = 2;
- second_spline_goal->spline.spline_idx = 2;
- second_spline_goal->spline.spline_count = 1;
- second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- second_spline_goal.Send();
- RunIteration();
+ // Second spline.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 2;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(dt());
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_start_goal->controller_type = 2;
- second_start_goal->spline_handle = 2;
- second_start_goal.Send();
+ // And then start it.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 2;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
// Tests that a second spline will run if the first is stopped.
TEST_F(DrivetrainTest, SplineStopFirst) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- goal->spline_handle = 1;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(1000));
+ RunFor(chrono::milliseconds(1000));
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
- my_drivetrain_queue_.goal.MakeMessage();
- stop_goal->controller_type = 2;
- stop_goal->spline_handle = 0;
- stop_goal.Send();
- RunForTime(chrono::milliseconds(500));
+ // Stop goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(500));
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_spline_goal->controller_type = 2;
- second_spline_goal->spline.spline_idx = 2;
- second_spline_goal->spline.spline_count = 1;
- second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- second_spline_goal->spline_handle = 2;
- second_spline_goal.Send();
+ // Second spline goal.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 2;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ message->spline_handle = 2;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
WaitForTrajectoryExecution();
@@ -720,37 +738,42 @@
// Tests that we can run a second spline after having planned but never executed
// the first spline.
TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
- // Don't start running the splane.
- goal->spline_handle = 0;
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ // Don't start running the splane.
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(1000));
+ RunFor(chrono::milliseconds(1000));
// Plan another spline, but don't start it yet:
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_spline_goal->controller_type = 2;
- second_spline_goal->spline.spline_idx = 2;
- second_spline_goal->spline.spline_count = 1;
- second_spline_goal->spline.spline_x = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
- second_spline_goal->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
- second_spline_goal->spline_handle = 0;
- second_spline_goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 2;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
+ message->spline_handle = 0;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- execute_goal = my_drivetrain_queue_.goal.MakeMessage();
- execute_goal->controller_type = 2;
- execute_goal->spline_handle = 2;
- execute_goal.Send();
+ // Now execute it.
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 2;
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryExecution();
VerifyNearSplineGoal();
@@ -759,77 +782,90 @@
// Tests that splines can excecute and plan at the same time.
TEST_F(DrivetrainTest, ParallelSplines) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_spline_goal->spline_handle = 1;
- second_spline_goal->controller_type = 2;
- second_spline_goal->spline.spline_idx = 2;
- second_spline_goal->spline.spline_count = 1;
- second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- second_spline_goal.Send();
+ // Second spline goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->spline_handle = 1;
+ message->controller_type = 2;
+ message->spline.spline_idx = 2;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryExecution();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
- second_start_goal->controller_type = 2;
- second_start_goal->spline_handle = 2;
- second_start_goal.Send();
+ // Second start goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 2;
+ EXPECT_TRUE(message.Send());
+ }
- RunForTime(chrono::milliseconds(4000));
+ RunFor(chrono::milliseconds(4000));
VerifyNearSplineGoal();
}
-//Tests that a trajectory never told to execute will not replan.
+// Tests that a trajectory never told to execute will not replan.
TEST_F(DrivetrainTest, OnlyPlanSpline) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
for (int i = 0; i < 100; ++i) {
- RunIteration();
- my_drivetrain_queue_.status.FetchLatest();
- EXPECT_EQ(my_drivetrain_queue_.status->trajectory_logging.planning_state, 3);
+ RunFor(dt());
+ drivetrain_status_fetcher_.Fetch();
+ EXPECT_EQ(drivetrain_status_fetcher_->trajectory_logging.planning_state,
+ 3);
::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
}
VerifyNearSplineGoal();
}
-//Tests that a trajectory can be executed after it is planned.
+// Tests that a trajectory can be executed after it is planned.
TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 2;
- goal->spline.spline_idx = 1;
- goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- goal.Send();
+ SetEnabled(true);
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline.spline_idx = 1;
+ message->spline.spline_count = 1;
+ message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ EXPECT_TRUE(message.Send());
+ }
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ RunFor(chrono::milliseconds(2000));
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
- start_goal = my_drivetrain_queue_.goal.MakeMessage();
- start_goal->controller_type = 2;
- start_goal->spline_handle = 1;
- start_goal.Send();
- RunForTime(chrono::milliseconds(2000));
+ // Start goal
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 2;
+ message->spline_handle = 1;
+ EXPECT_TRUE(message.Send());
+ }
+ RunFor(chrono::milliseconds(2000));
VerifyNearPosition(1.0, 1.0);
}
@@ -837,58 +873,66 @@
// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
// tests that the integration with drivetrain_lib worked properly.
TEST_F(DrivetrainTest, BasicLineFollow) {
+ SetEnabled(true);
localizer_.target_selector()->set_has_target(true);
localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 3;
- goal->throttle = 0.5;
- goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 3;
+ message->throttle = 0.5;
+ EXPECT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
- my_drivetrain_queue_.status.FetchLatest();
- EXPECT_TRUE(my_drivetrain_queue_.status->line_follow_logging.frozen);
- EXPECT_TRUE(my_drivetrain_queue_.status->line_follow_logging.have_target);
- EXPECT_EQ(1.0, my_drivetrain_queue_.status->line_follow_logging.x);
- EXPECT_EQ(1.0, my_drivetrain_queue_.status->line_follow_logging.y);
+ drivetrain_status_fetcher_.Fetch();
+ EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.frozen);
+ EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.have_target);
+ EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.x);
+ EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.y);
EXPECT_FLOAT_EQ(M_PI_4,
- my_drivetrain_queue_.status->line_follow_logging.theta);
+ drivetrain_status_fetcher_->line_follow_logging.theta);
// Should have run off the end of the target, running along the y=x line.
- EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
- EXPECT_NEAR(drivetrain_motor_plant_.GetPosition().x(),
- drivetrain_motor_plant_.GetPosition().y(), 0.1);
+ EXPECT_LT(1.0, drivetrain_plant_.GetPosition().x());
+ EXPECT_NEAR(drivetrain_plant_.GetPosition().x(),
+ drivetrain_plant_.GetPosition().y(), 0.1);
}
// Tests that the line follower will not run and defer to regular open-loop
// driving when there is no target yet:
TEST_F(DrivetrainTest, LineFollowDefersToOpenLoop) {
+ SetEnabled(true);
localizer_.target_selector()->set_has_target(false);
localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
- my_drivetrain_queue_.goal.MakeMessage();
- goal->controller_type = 3;
- goal->throttle = 0.5;
- goal.Send();
+ {
+ auto message = drivetrain_goal_sender_.MakeMessage();
+ message->controller_type = 3;
+ message->throttle = 0.5;
+ EXPECT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
// Should have run straight (because we just set throttle, with wheel = 0)
// along X-axis.
- EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
- EXPECT_NEAR(0.0, drivetrain_motor_plant_.GetPosition().y(), 1e-4);
+ EXPECT_LT(1.0, drivetrain_plant_.GetPosition().x());
+ EXPECT_NEAR(0.0, drivetrain_plant_.GetPosition().y(), 1e-4);
}
// Tests that we can reset the localizer to a new position.
TEST_F(DrivetrainTest, ResetLocalizer) {
+ SetEnabled(true);
EXPECT_EQ(0.0, localizer_.x());
EXPECT_EQ(0.0, localizer_.y());
EXPECT_EQ(0.0, localizer_.theta());
- ::aos::Queue<LocalizerControl> localizer_queue(
- ".frc971.control_loops.drivetrain.localizer_control");
- ASSERT_TRUE(
- localizer_queue.MakeWithBuilder().x(9.0).y(7.0).theta(1.0).Send());
- RunIteration();
+ {
+ auto message = localizer_control_sender_.MakeMessage();
+ message->x = 9.0;
+ message->y = 7.0;
+ message->theta = 1.0;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(dt());
EXPECT_EQ(9.0, localizer_.x());
EXPECT_EQ(7.0, localizer_.y());
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index a5cf6af..34c9907 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -6,12 +6,18 @@
#include "frc971/control_loops/drivetrain/trajectory.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "gflags/gflags.h"
+#if defined(SUPPORT_PLOT)
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#endif
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+DEFINE_bool(plot, false, "If true, plot");
+
namespace frc971 {
namespace control_loops {
namespace drivetrain {
@@ -86,16 +92,23 @@
: event_loop_(event_loop),
robot_state_fetcher_(
event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state")),
- dt_config_(dt_config),
- drivetrain_plant_(MakePlantFromConfig(dt_config_)),
- my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
- ".frc971.control_loops.drivetrain_queue.goal",
- ".frc971.control_loops.drivetrain_queue.position",
- ".frc971.control_loops.drivetrain_queue.output",
- ".frc971.control_loops.drivetrain_queue.status"),
+ drivetrain_position_sender_(
+ event_loop_
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")),
+ drivetrain_output_fetcher_(
+ event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".frc971.control_loops.drivetrain_queue.output")),
+ drivetrain_status_fetcher_(
+ event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
gyro_reading_sender_(
event_loop->MakeSender<::frc971::sensors::GyroReading>(
".frc971.sensors.gyro_reading")),
+ dt_config_(dt_config),
+ drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
::std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
StateFeedbackHybridPlant<2, 2, 2>,
@@ -106,6 +119,29 @@
dt_config_.make_hybrid_drivetrain_velocity_loop()))) {
Reinitialize();
last_U_.setZero();
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ if (FLAGS_plot) {
+ EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+
+ ::Eigen::Matrix<double, 2, 1> actual_position = GetPosition();
+ actual_x_.push_back(actual_position(0));
+ actual_y_.push_back(actual_position(1));
+
+ trajectory_x_.push_back(
+ drivetrain_status_fetcher_->trajectory_logging.x);
+ trajectory_y_.push_back(
+ drivetrain_status_fetcher_->trajectory_logging.y);
+ }
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt_config_.dt);
}
void DrivetrainSimulation::Reinitialize() {
@@ -123,8 +159,8 @@
const double right_encoder = GetRightPosition();
{
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Position>
- position = my_drivetrain_queue_.position.MakeMessage();
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>::Message
+ position = drivetrain_position_sender_.MakeMessage();
position->left_encoder = left_encoder;
position->right_encoder = right_encoder;
position->left_shifter_position = left_gear_high_ ? 1.0 : 0.0;
@@ -146,10 +182,10 @@
void DrivetrainSimulation::Simulate() {
last_left_position_ = drivetrain_plant_.Y(0, 0);
last_right_position_ = drivetrain_plant_.Y(1, 0);
- EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ EXPECT_TRUE(drivetrain_output_fetcher_.Fetch());
::Eigen::Matrix<double, 2, 1> U = last_U_;
- last_U_ << my_drivetrain_queue_.output->left_voltage,
- my_drivetrain_queue_.output->right_voltage;
+ last_U_ << drivetrain_output_fetcher_->left_voltage,
+ drivetrain_output_fetcher_->right_voltage;
{
robot_state_fetcher_.Fetch();
const double scalar = robot_state_fetcher_.get()
@@ -157,8 +193,8 @@
: 1.0;
last_U_ *= scalar;
}
- left_gear_high_ = my_drivetrain_queue_.output->left_high;
- right_gear_high_ = my_drivetrain_queue_.output->right_high;
+ left_gear_high_ = drivetrain_output_fetcher_->left_high;
+ right_gear_high_ = drivetrain_output_fetcher_->right_high;
if (left_gear_high_) {
if (right_gear_high_) {
@@ -187,6 +223,20 @@
state_, U, dt_float);
}
+void DrivetrainSimulation::MaybePlot() {
+#if defined(SUPPORT_PLOT)
+ if (FLAGS_plot) {
+ std::cout << "Plotting." << ::std::endl;
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(actual_x_, actual_y_, {{"label", "actual position"}});
+ matplotlibcpp::plot(trajectory_x_, trajectory_y_,
+ {{"label", "trajectory position"}});
+ matplotlibcpp::legend();
+ matplotlibcpp::show();
+ }
+#endif
+}
+
} // namespace testing
} // namespace drivetrain
} // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index cdde176..9a4f177 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -52,12 +52,6 @@
double GetLeftPosition() const { return drivetrain_plant_.Y(0, 0); }
double GetRightPosition() const { return drivetrain_plant_.Y(1, 0); }
- // Sends out the position queue messages.
- void SendPositionMessage();
-
- // Simulates the drivetrain moving for one timestep.
- void Simulate();
-
void set_left_voltage_offset(double left_voltage_offset) {
drivetrain_plant_.set_left_voltage_offset(left_voltage_offset);
}
@@ -73,17 +67,30 @@
return state_.block<2,1>(0,0);
}
+ void MaybePlot();
+
private:
+ // Sends out the position queue messages.
+ void SendPositionMessage();
+
+ // Simulates the drivetrain moving for one timestep.
+ void Simulate();
+
::aos::EventLoop *event_loop_;
::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ drivetrain_output_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
+
DrivetrainConfig<double> dt_config_;
DrivetrainPlant drivetrain_plant_;
- ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
- ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
-
// This state is [x, y, theta, left_velocity, right_velocity].
::Eigen::Matrix<double, 5, 1> state_ = ::Eigen::Matrix<double, 5, 1>::Zero();
::std::unique_ptr<
@@ -96,6 +103,12 @@
bool left_gear_high_ = false;
bool right_gear_high_ = false;
+ bool first_ = true;
+
+ ::std::vector<double> actual_x_;
+ ::std::vector<double> actual_y_;
+ ::std::vector<double> trajectory_x_;
+ ::std::vector<double> trajectory_y_;
};
} // namespace testing
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 68f67f6..16b0b61 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -488,6 +488,9 @@
// tuning things. I haven't yet tried messing with these values again.
encoder_noise_ = 0.5 * R_kf_drivetrain(0, 0);
gyro_noise_ = 0.1 * R_kf_drivetrain(2, 2);
+
+ X_hat_.setZero();
+ P_.setZero();
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 7d401d6..f4d718e 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -9,7 +9,7 @@
#include "gtest/gtest.h"
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
-DEFINE_bool(plot, false, "If true, plot");
+DECLARE_bool(plot);
namespace chrono = ::std::chrono;
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 23a978c..7b2ceae 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -1,6 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
+#include "aos/events/event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/pose.h"
@@ -94,18 +95,22 @@
class DeadReckonEkf : public LocalizerInterface {
typedef HybridEkf<double> Ekf;
typedef typename Ekf::StateIdx StateIdx;
+
public:
- DeadReckonEkf(const DrivetrainConfig<double> &dt_config) : ekf_(dt_config) {
- ekf_.ResetInitialState(::aos::monotonic_clock::now(), Ekf::State::Zero(),
- ekf_.P());
+ DeadReckonEkf(::aos::EventLoop *event_loop,
+ const DrivetrainConfig<double> &dt_config)
+ : ekf_(dt_config) {
+ event_loop->OnRun([this, event_loop]() {
+ ekf_.ResetInitialState(event_loop->monotonic_now(), Ekf::State::Zero(),
+ ekf_.P());
+ });
target_selector_.set_has_target(false);
}
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
- ::aos::monotonic_clock::time_point now,
- double left_encoder, double right_encoder,
- double gyro_rate,
- double /*longitudinal_accelerometer*/) override {
+ ::aos::monotonic_clock::time_point now, double left_encoder,
+ double right_encoder, double gyro_rate,
+ double /*longitudinal_accelerometer*/) override {
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
}
@@ -115,7 +120,8 @@
const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
- right_encoder, 0, 0, 0, 0).finished(),
+ right_encoder, 0, 0, 0,
+ 0).finished(),
ekf_.P());
};
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index 1b6b965..ac23907 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -195,4 +195,4 @@
struct StaticZeroingSingleDOFProfiledSubsystemGoal {
double unsafe_goal;
.frc971.ProfileParameters profile_params;
-};
\ No newline at end of file
+};
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 de9c22b..58096f3 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,9 +1,11 @@
#include "gtest/gtest.h"
+#include "aos/controls/control_loop.h"
#include "aos/controls/control_loop_test.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_plant.h"
@@ -25,7 +27,14 @@
zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
-typedef ::testing::Types<SZSDPS_AbsEncoder, SZSDPS_PotAndAbsEncoder> TestTypes;
+typedef ::testing::Types<
+ ::std::pair<
+ SZSDPS_AbsEncoder,
+ StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>,
+ ::std::pair<
+ SZSDPS_PotAndAbsEncoder,
+ StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>>
+ TestTypes;
constexpr ::frc971::constants::Range kRange{
.lower_hard = -0.01, .upper_hard = 0.250, .lower = 0.01, .upper = 0.235};
@@ -69,170 +78,78 @@
return params;
}
-template <typename ZeroingEstimator, typename ProfiledJointStatus>
-struct TestIntakeSystemData {
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal goal;
-
- ProfiledJointStatus status;
-
- typename ZeroingEstimator::Position position;
-
- double output;
-};
-
} // namespace
-template <typename SZSDPS>
+template <typename SZSDPS, typename QueueGroup>
class TestIntakeSystemSimulation {
public:
- TestIntakeSystemSimulation()
- : subsystem_plant_(new CappedTestPlant(
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
+ PositionType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
+ StatusType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
+ OutputType;
+
+ TestIntakeSystemSimulation(::aos::EventLoop *event_loop,
+ chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ subsystem_position_sender_(
+ event_loop_->MakeSender<PositionType>(".position")),
+ subsystem_status_fetcher_(
+ event_loop_->MakeFetcher<StatusType>(".status")),
+ subsystem_output_fetcher_(
+ event_loop_->MakeFetcher<OutputType>(".output")),
+ subsystem_plant_(new CappedTestPlant(
::frc971::control_loops::MakeTestIntakeSystemPlant())),
subsystem_sensor_sim_(kEncoderIndexDifference) {
// Start the subsystem out in the middle by default.
InitializeSubsystemPosition((kRange.lower + kRange.upper) / 2.0);
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ this->Simulate();
+ }
+ first_ = false;
+ this->SendPositionMessage();
+ },
+ dt);
}
void InitializeSubsystemPosition(double start_pos) {
- subsystem_plant_->mutable_X(0, 0) = start_pos;
- subsystem_plant_->mutable_X(1, 0) = 0.0;
+ this->subsystem_plant_->mutable_X(0, 0) = start_pos;
+ this->subsystem_plant_->mutable_X(1, 0) = 0.0;
- InitializeSensorSim(start_pos);
+ this->InitializeSensorSim(start_pos);
}
void InitializeSensorSim(double start_pos);
- // Updates the position message with the position of the subsystem.
- void UpdatePositionMessage(
- typename SZSDPS::ZeroingEstimator::Position *position) {
- subsystem_sensor_sim_.GetSensorValues(position);
- }
-
- double subsystem_position() const { return subsystem_plant_->X(0, 0); }
- double subsystem_velocity() const { return subsystem_plant_->X(1, 0); }
+ double subsystem_position() const { return this->subsystem_plant_->X(0, 0); }
+ double subsystem_velocity() const { return this->subsystem_plant_->X(1, 0); }
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
void set_subsystem_voltage_offset(double voltage_offset) {
- subsystem_plant_->set_voltage_offset(voltage_offset);
+ this->subsystem_plant_->set_voltage_offset(voltage_offset);
}
- // Simulates the subsystem for a single timestep.
- void Simulate(const double output_voltage, const int32_t state) {
- const double voltage_check_subsystem =
- (static_cast<typename SZSDPS::State>(state) == SZSDPS::State::RUNNING)
- ? kOperatingVoltage
- : kZeroingVoltage;
+ // Sends a queue message with the position.
+ void SendPositionMessage() {
+ typename ::aos::Sender<PositionType>::Message position =
+ subsystem_position_sender_.MakeMessage();
- EXPECT_LE(::std::abs(output_voltage), voltage_check_subsystem);
+ this->subsystem_sensor_sim_.GetSensorValues(&position->position);
- ::Eigen::Matrix<double, 1, 1> subsystem_U;
- subsystem_U << output_voltage + subsystem_plant_->voltage_offset();
- subsystem_plant_->Update(subsystem_U);
-
- const double position_subsystem = subsystem_plant_->Y(0, 0);
-
- subsystem_sensor_sim_.MoveTo(position_subsystem);
-
- EXPECT_GE(position_subsystem, kRange.lower_hard);
- EXPECT_LE(position_subsystem, kRange.upper_hard);
- }
-
- private:
- ::std::unique_ptr<CappedTestPlant> subsystem_plant_;
- PositionSensorSimulator subsystem_sensor_sim_;
-};
-
-template <>
-void TestIntakeSystemSimulation<SZSDPS_PotAndAbsEncoder>::InitializeSensorSim(
- double start_pos) {
- subsystem_sensor_sim_.Initialize(
- start_pos, kNoiseScalar, 0.0,
- TestIntakeSystemValues<
- typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
- .measured_absolute_position);
-}
-
-template <>
-void TestIntakeSystemSimulation<SZSDPS_AbsEncoder>::InitializeSensorSim(
- double start_pos) {
- subsystem_sensor_sim_.Initialize(
- start_pos, kNoiseScalar, 0.0,
- TestIntakeSystemValues<
- typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
- .measured_absolute_position);
-}
-
-template <typename TSZSDPS>
-class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
- protected:
- using SZSDPS = TSZSDPS;
- using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
- using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
-
- IntakeSystemTest()
- : robot_state_fetcher_(
- simulation_event_loop_.MakeFetcher<::aos::RobotState>(
- ".aos.robot_state")),
- subsystem_(TestIntakeSystemValues<
- typename SZSDPS::ZeroingEstimator>::make_params()),
- subsystem_plant_() {}
-
- void VerifyNearGoal() {
- EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
- subsystem_data_.status.position, 0.001);
- EXPECT_NEAR(subsystem_data_.goal.unsafe_goal,
- subsystem_plant_.subsystem_position(), 0.001);
- EXPECT_NEAR(subsystem_data_.status.velocity, 0, 0.001);
- }
-
- // Runs one iteration of the whole simulation.
- void RunIteration(bool enabled = true, bool null_goal = false) {
- SendMessages(enabled);
- subsystem_plant_.UpdatePositionMessage(&subsystem_data_.position);
-
- // Checks if the robot was reset and resets the subsystem.
- // Required since there is no ControlLoop to reset it (ie. a superstructure)
- robot_state_fetcher_.Fetch();
- if (robot_state_fetcher_.get()) {
- if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid) {
- LOG(ERROR, "WPILib reset, restarting\n");
- subsystem_.Reset();
- }
- sensor_reader_pid_ = robot_state_fetcher_->reader_pid;
- }
- subsystem_goal_.unsafe_goal = subsystem_data_.goal.unsafe_goal;
- subsystem_goal_.profile_params = subsystem_data_.goal.profile_params;
-
- subsystem_.Iterate(null_goal ? nullptr : &subsystem_goal_,
- &subsystem_data_.position, &subsystem_data_.output,
- &subsystem_data_.status);
-
- subsystem_plant_.Simulate(subsystem_data_.output,
- subsystem_data_.status.state);
-
- TickTime(::std::chrono::microseconds(5050));
- }
-
- // Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const monotonic_clock::duration run_for, bool enabled = true,
- bool null_goal = false) {
- const auto start_time = monotonic_clock::now();
- while (monotonic_clock::now() < start_time + run_for) {
- const auto loop_start_time = monotonic_clock::now();
- double begin_subsystem_velocity = subsystem_plant_.subsystem_velocity();
-
- RunIteration(enabled, null_goal);
-
- const double loop_time = ::aos::time::DurationInSeconds(
- monotonic_clock::now() - loop_start_time);
- const double subsystem_acceleration =
- (subsystem_plant_.subsystem_velocity() - begin_subsystem_velocity) /
- loop_time;
- EXPECT_NEAR(subsystem_acceleration, 0.0, peak_subsystem_acceleration_);
- EXPECT_NEAR(subsystem_plant_.subsystem_velocity(), 0.0,
- peak_subsystem_velocity_);
- }
+ position.Send();
}
void set_peak_subsystem_acceleration(double value) {
@@ -242,53 +159,233 @@
peak_subsystem_velocity_ = value;
}
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointer to shared memory
- // that is no longer valid.
- // TestIntakeSystemData subsystem_data_;
- ::aos::ShmEventLoop simulation_event_loop_;
- ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+ // Simulates the subsystem for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(subsystem_output_fetcher_.Fetch());
+ EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
- // Create a control loop and simulation.
- SZSDPS subsystem_;
- TestIntakeSystemSimulation<SZSDPS> subsystem_plant_;
+ const double begin_subsystem_velocity = subsystem_velocity();
- StaticZeroingSingleDOFProfiledSubsystemGoal subsystem_goal_;
+ const double voltage_check_subsystem =
+ (static_cast<typename SZSDPS::State>(
+ subsystem_status_fetcher_->status.state) == SZSDPS::State::RUNNING)
+ ? kOperatingVoltage
+ : kZeroingVoltage;
- TestIntakeSystemData<typename SZSDPS::ZeroingEstimator,
- typename SZSDPS::ProfiledJointStatus>
- subsystem_data_;
+ EXPECT_LE(::std::abs(subsystem_output_fetcher_->output),
+ voltage_check_subsystem);
+
+ ::Eigen::Matrix<double, 1, 1> subsystem_U;
+ subsystem_U << subsystem_output_fetcher_->output +
+ subsystem_plant_->voltage_offset();
+ subsystem_plant_->Update(subsystem_U);
+
+ const double position_subsystem = subsystem_plant_->Y(0, 0);
+
+ subsystem_sensor_sim_.MoveTo(position_subsystem);
+
+ EXPECT_GE(position_subsystem, kRange.lower_hard);
+ EXPECT_LE(position_subsystem, kRange.upper_hard);
+
+ const double loop_time = ::aos::time::DurationInSeconds(dt_);
+ const double subsystem_acceleration =
+ (subsystem_velocity() - begin_subsystem_velocity) / loop_time;
+ EXPECT_NEAR(subsystem_acceleration, 0.0, peak_subsystem_acceleration_);
+ EXPECT_NEAR(subsystem_velocity(), 0.0, peak_subsystem_velocity_);
+ }
private:
+ ::aos::EventLoop *event_loop_;
+ chrono::nanoseconds dt_;
+
+ bool first_ = true;
+
+ typename ::aos::Sender<PositionType> subsystem_position_sender_;
+ typename ::aos::Fetcher<StatusType> subsystem_status_fetcher_;
+ typename ::aos::Fetcher<OutputType> subsystem_output_fetcher_;
+
+ ::std::unique_ptr<CappedTestPlant> subsystem_plant_;
+ PositionSensorSimulator subsystem_sensor_sim_;
+
// The acceleration limits to check for while moving.
double peak_subsystem_acceleration_ = 1e10;
// The velocity limits to check for while moving.
double peak_subsystem_velocity_ = 1e10;
+};
- int32_t sensor_reader_pid_ = 0;
+template <>
+void TestIntakeSystemSimulation<
+ SZSDPS_PotAndAbsEncoder,
+ StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>::
+ InitializeSensorSim(double start_pos) {
+ subsystem_sensor_sim_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues<
+ typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+ .measured_absolute_position);
+}
+
+template <>
+void TestIntakeSystemSimulation<
+ SZSDPS_AbsEncoder,
+ StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>::
+ InitializeSensorSim(double start_pos) {
+ subsystem_sensor_sim_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ TestIntakeSystemValues<
+ typename SZSDPS_PotAndAbsEncoder::ZeroingEstimator>::kZeroing
+ .measured_absolute_position);
+}
+
+// Class to represent a module using a subsystem. This lets us use event loops
+// to wrap it.
+template <typename QueueGroup, typename SZSDPS>
+class Subsystem : public ::aos::controls::ControlLoop<QueueGroup> {
+ public:
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
+ PositionType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
+ StatusType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
+ OutputType;
+
+ Subsystem(::aos::EventLoop *event_loop, const ::std::string &name)
+ : aos::controls::ControlLoop<QueueGroup>(event_loop, name),
+ subsystem_(TestIntakeSystemValues<
+ typename SZSDPS::ZeroingEstimator>::make_params()) {}
+
+ void RunIteration(const GoalType *unsafe_goal, const PositionType *position,
+ OutputType *output, StatusType *status) {
+ if (this->WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ subsystem_.Reset();
+ }
+
+ // Convert one goal type to another...
+ StaticZeroingSingleDOFProfiledSubsystemGoal goal;
+ if (unsafe_goal != nullptr ) {
+ goal.unsafe_goal = unsafe_goal->unsafe_goal;
+ goal.profile_params.max_velocity =
+ unsafe_goal->profile_params.max_velocity;
+ goal.profile_params.max_acceleration =
+ unsafe_goal->profile_params.max_acceleration;
+ }
+
+ subsystem_.Iterate(
+ unsafe_goal == nullptr ? nullptr : &goal, &position->position,
+ output == nullptr ? nullptr : &output->output, &status->status);
+ }
+
+ SZSDPS *subsystem() { return &subsystem_; }
+
+ private:
+ SZSDPS subsystem_;
+};
+
+template <typename TSZSDPS>
+class IntakeSystemTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ using SZSDPS = typename TSZSDPS::first_type;
+ using QueueGroup = typename TSZSDPS::second_type;
+ using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
+ using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
+
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
+ PositionType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
+ StatusType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
+ OutputType;
+
+ IntakeSystemTest()
+ : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ test_event_loop_(MakeEventLoop()),
+ subsystem_goal_sender_(test_event_loop_->MakeSender<GoalType>(".goal")),
+ subsystem_goal_fetcher_(
+ test_event_loop_->MakeFetcher<GoalType>(".goal")),
+ subsystem_status_fetcher_(
+ test_event_loop_->MakeFetcher<StatusType>(".status")),
+ subsystem_event_loop_(MakeEventLoop()),
+ subsystem_(subsystem_event_loop_.get(), ""),
+ subsystem_plant_event_loop_(MakeEventLoop()),
+ subsystem_plant_(subsystem_plant_event_loop_.get(), dt()) {}
+
+ void VerifyNearGoal() {
+ subsystem_goal_fetcher_.Fetch();
+ EXPECT_TRUE(subsystem_goal_fetcher_.get() != nullptr);
+ EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
+ subsystem_status_fetcher_->status.position, 0.001);
+ EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
+ subsystem_plant_.subsystem_position(), 0.001);
+ EXPECT_NEAR(subsystem_status_fetcher_->status.velocity, 0, 0.001);
+ }
+
+ SZSDPS *subsystem() { return subsystem_.subsystem(); }
+
+ void set_peak_subsystem_acceleration(double value) {
+ set_peak_subsystem_acceleration(value);
+ }
+ void set_peak_subsystem_velocity(double value) {
+ set_peak_subsystem_velocity(value);
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Sender<GoalType> subsystem_goal_sender_;
+ ::aos::Fetcher<GoalType> subsystem_goal_fetcher_;
+ ::aos::Fetcher<StatusType> subsystem_status_fetcher_;
+
+ // Create a control loop and simulation.
+ ::std::unique_ptr<::aos::EventLoop> subsystem_event_loop_;
+ Subsystem<QueueGroup, SZSDPS> subsystem_;
+
+ ::std::unique_ptr<::aos::EventLoop> subsystem_plant_event_loop_;
+ TestIntakeSystemSimulation<SZSDPS, QueueGroup> subsystem_plant_;
};
TYPED_TEST_CASE_P(IntakeSystemTest);
// Tests that the subsystem does nothing when the goal is zero.
TYPED_TEST_P(IntakeSystemTest, DoesNothing) {
+ this->SetEnabled(true);
// Intake system uses 0.05 to test for 0.
- this->subsystem_data_.goal.unsafe_goal = 0.05;
- this->RunForTime(chrono::seconds(5));
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = 0.05;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(5));
this->VerifyNearGoal();
}
// Tests that the subsystem loop can reach a goal.
TYPED_TEST_P(IntakeSystemTest, ReachesGoal) {
+ this->SetEnabled(true);
// Set a reasonable goal.
- auto &goal = this->subsystem_data_.goal;
- goal.unsafe_goal = 0.1;
- goal.profile_params.max_velocity = 1;
- goal.profile_params.max_acceleration = 0.5;
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = 0.1;
+ message->profile_params.max_velocity = 1;
+ message->profile_params.max_acceleration = 0.5;
+ EXPECT_TRUE(message.Send());
+ }
// Give it a lot of time to get there.
- this->RunForTime(chrono::seconds(8));
+ this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
}
@@ -296,35 +393,43 @@
// Makes sure that the voltage on a motor is properly pulled back after
// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
TYPED_TEST_P(IntakeSystemTest, SaturationTest) {
+ this->SetEnabled(true);
// Zero it before we move.
- auto &goal = this->subsystem_data_.goal;
- goal.unsafe_goal = kRange.upper;
- this->RunForTime(chrono::seconds(8));
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- goal.unsafe_goal = kRange.lower;
- goal.profile_params.max_velocity = 20.0;
- goal.profile_params.max_acceleration = 0.1;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.lower;
+ message->profile_params.max_velocity = 20.0;
+ message->profile_params.max_acceleration = 0.1;
+ EXPECT_TRUE(message.Send());
}
this->set_peak_subsystem_velocity(23.0);
this->set_peak_subsystem_acceleration(0.2);
- this->RunForTime(chrono::seconds(8));
+ this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
// Now do a high acceleration move with a low velocity limit.
{
- goal.unsafe_goal = kRange.upper;
- goal.profile_params.max_velocity = 0.1;
- goal.profile_params.max_acceleration = 100;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ message->profile_params.max_velocity = 0.1;
+ message->profile_params.max_acceleration = 100;
+ EXPECT_TRUE(message.Send());
}
this->set_peak_subsystem_velocity(0.2);
this->set_peak_subsystem_acceleration(103);
- this->RunForTime(chrono::seconds(8));
+ this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
}
@@ -332,184 +437,244 @@
// Tests that the subsystem loop doesn't try and go beyond it's physical range
// of the mechanisms.
TYPED_TEST_P(IntakeSystemTest, RespectsRange) {
- auto &goal = this->subsystem_data_.goal;
+ this->SetEnabled(true);
+
// Set some ridiculous goals to test upper limits.
{
- goal.unsafe_goal = 100.0;
- goal.profile_params.max_velocity = 1;
- goal.profile_params.max_acceleration = 0.5;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = 100.0;
+ message->profile_params.max_velocity = 1;
+ message->profile_params.max_acceleration = 0.5;
+ EXPECT_TRUE(message.Send());
}
- this->RunForTime(chrono::seconds(10));
+ this->RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- EXPECT_NEAR(kRange.upper, this->subsystem_data_.status.position, 0.001);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ 0.001);
// Set some ridiculous goals to test lower limits.
{
- goal.unsafe_goal = -100.0;
- goal.profile_params.max_velocity = 1;
- goal.profile_params.max_acceleration = 0.5;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = -100.0;
+ message->profile_params.max_velocity = 1;
+ message->profile_params.max_acceleration = 0.5;
+ EXPECT_TRUE(message.Send());
}
- this->RunForTime(chrono::seconds(10));
+ this->RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- EXPECT_NEAR(kRange.lower, this->subsystem_data_.status.position, 0.001);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ 0.001);
}
// Tests that the subsystem loop zeroes when run for a while.
TYPED_TEST_P(IntakeSystemTest, ZeroTest) {
- auto &goal = this->subsystem_data_.goal;
+ this->SetEnabled(true);
+
{
- goal.unsafe_goal = kRange.upper;
- goal.profile_params.max_velocity = 1;
- goal.profile_params.max_acceleration = 0.5;
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ message->profile_params.max_velocity = 1;
+ message->profile_params.max_acceleration = 0.5;
+ EXPECT_TRUE(message.Send());
}
- this->RunForTime(chrono::seconds(10));
+ this->RunFor(chrono::seconds(10));
this->VerifyNearGoal();
}
// Tests that the loop zeroes when run for a while without a goal.
TYPED_TEST_P(IntakeSystemTest, ZeroNoGoal) {
- this->RunForTime(chrono::seconds(5));
+ this->SetEnabled(true);
+ this->RunFor(chrono::seconds(5));
- EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
}
TYPED_TEST_P(IntakeSystemTest, LowerHardstopStartup) {
+ this->SetEnabled(true);
this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
- this->subsystem_data_.goal.unsafe_goal = kRange.upper;
- this->RunForTime(chrono::seconds(10));
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(10));
this->VerifyNearGoal();
}
// Tests that starting at the upper hardstops doesn't cause an abort.
TYPED_TEST_P(IntakeSystemTest, UpperHardstopStartup) {
+ this->SetEnabled(true);
+
this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
- this->subsystem_data_.goal.unsafe_goal = kRange.upper;
- this->RunForTime(chrono::seconds(10));
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(10));
this->VerifyNearGoal();
}
// Tests that resetting WPILib results in a rezero.
TYPED_TEST_P(IntakeSystemTest, ResetTest) {
- this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
- this->subsystem_data_.goal.unsafe_goal = kRange.upper - 0.1;
- this->RunForTime(chrono::seconds(10));
+ this->SetEnabled(true);
- EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper - 0.1;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(10));
+
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
this->VerifyNearGoal();
this->SimulateSensorReset();
- this->RunForTime(chrono::milliseconds(100));
+ this->RunFor(chrono::milliseconds(100));
EXPECT_EQ(TestFixture::SZSDPS::State::UNINITIALIZED,
- this->subsystem_.state());
+ this->subsystem()->state());
- this->RunForTime(chrono::seconds(10));
+ this->RunFor(chrono::seconds(10));
- EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
this->VerifyNearGoal();
}
// Tests that the internal goals don't change while disabled.
TYPED_TEST_P(IntakeSystemTest, DisabledGoalTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.lower + 0.03;
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.lower + 0.03;
+ EXPECT_TRUE(message.Send());
+ }
// Checks that the subsystem has not moved from its starting position at 0
- this->RunForTime(chrono::milliseconds(100), false);
- EXPECT_EQ(0.0, this->subsystem_.goal(0));
+ this->RunFor(chrono::milliseconds(100));
+ EXPECT_EQ(0.0, this->subsystem()->goal(0));
// Now make sure they move correctly
- this->RunForTime(chrono::seconds(4), true);
- EXPECT_NE(0.0, this->subsystem_.goal(0));
+ this->SetEnabled(true);
+ this->RunFor(chrono::seconds(4));
+ EXPECT_NE(0.0, this->subsystem()->goal(0));
}
// Tests that zeroing while disabled works.
TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.lower;
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.lower;
+ EXPECT_TRUE(message.Send());
+ }
// Run disabled for 2 seconds
- this->RunForTime(chrono::seconds(2), false);
- EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem_.state());
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(TestFixture::SZSDPS::State::RUNNING, this->subsystem()->state());
- this->RunForTime(chrono::seconds(4), true);
+ this->SetEnabled(true);
+ this->RunFor(chrono::seconds(4));
this->VerifyNearGoal();
}
// Tests that set_min_position limits range properly
TYPED_TEST_P(IntakeSystemTest, MinPositionTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.lower_hard;
- this->RunForTime(chrono::seconds(2), true);
+ this->SetEnabled(true);
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.lower_hard;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(2));
// Check that kRange.lower is used as the default min position
- EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower, 0.001);
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ 0.001);
// Set min position and check that the subsystem increases to that position
- this->subsystem_.set_min_position(kRange.lower + 0.05);
- this->RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(this->subsystem_.goal(0), kRange.lower + 0.05);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower + 0.05,
+ this->subsystem()->set_min_position(kRange.lower + 0.05);
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.lower + 0.05);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.lower + 0.05, this->subsystem_status_fetcher_->status.position,
0.001);
// Clear min position and check that the subsystem returns to kRange.lower
- this->subsystem_.clear_min_position();
- this->RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(this->subsystem_.goal(0), kRange.lower);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.lower, 0.001);
+ this->subsystem()->clear_min_position();
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ 0.001);
}
// Tests that set_max_position limits range properly
TYPED_TEST_P(IntakeSystemTest, MaxPositionTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.upper_hard;
- this->RunForTime(chrono::seconds(2), true);
+ this->SetEnabled(true);
+
+ {
+ auto message = this->subsystem_goal_sender_.MakeMessage();
+ message->unsafe_goal = kRange.upper_hard;
+ EXPECT_TRUE(message.Send());
+ }
+ this->RunFor(chrono::seconds(2));
// Check that kRange.upper is used as the default max position
- EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper, 0.001);
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ 0.001);
// Set max position and check that the subsystem lowers to that position
- this->subsystem_.set_max_position(kRange.upper - 0.05);
- this->RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(this->subsystem_.goal(0), kRange.upper - 0.05);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper - 0.05,
+ this->subsystem()->set_max_position(kRange.upper - 0.05);
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.upper - 0.05);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.upper - 0.05, this->subsystem_status_fetcher_->status.position,
0.001);
// Clear max position and check that the subsystem returns to kRange.upper
- this->subsystem_.clear_max_position();
- this->RunForTime(chrono::seconds(2), true);
- EXPECT_EQ(this->subsystem_.goal(0), kRange.upper);
- EXPECT_NEAR(this->subsystem_data_.status.position, kRange.upper, 0.001);
+ this->subsystem()->clear_max_position();
+ this->RunFor(chrono::seconds(2));
+ EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ 0.001);
}
// Tests that the subsystem maintains its current position when sent a null goal
TYPED_TEST_P(IntakeSystemTest, NullGoalTest) {
- this->subsystem_data_.goal.unsafe_goal = kRange.lower + 0.05;
- this->RunForTime(chrono::seconds(2), true);
+ this->SetEnabled(true);
- this->VerifyNearGoal();
+ this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
- // Run with a null goal
- this->RunForTime(chrono::seconds(2), true, true);
+ this->RunFor(chrono::seconds(5));
- // Check that the subsystem has not moved
- this->VerifyNearGoal();
+ EXPECT_NEAR(kRange.upper, this->subsystem_plant_.subsystem_position(), 0.001);
+ EXPECT_NEAR(this->subsystem_plant_.subsystem_velocity(), 0, 0.001);
}
// Tests that the subsystem estops when a zeroing error occurs
TYPED_TEST_P(IntakeSystemTest, ZeroingErrorTest) {
- this->RunForTime(chrono::seconds(2), true);
+ this->SetEnabled(true);
+ this->RunFor(chrono::seconds(2));
- EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::RUNNING);
- this->subsystem_.TriggerEstimatorError();
- this->RunIteration(true, false);
- EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::ESTOP);
+ EXPECT_EQ(this->subsystem()->state(), TestFixture::SZSDPS::State::RUNNING);
+ this->subsystem()->TriggerEstimatorError();
+ this->RunFor(this->dt());
+ EXPECT_EQ(this->subsystem()->state(), TestFixture::SZSDPS::State::ESTOP);
}
REGISTER_TYPED_TEST_CASE_P(IntakeSystemTest, DoesNothing, ReachesGoal,
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
new file mode 100644
index 0000000..3b35837
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
@@ -0,0 +1,46 @@
+package frc971.control_loops;
+
+import "frc971/control_loops/profiled_subsystem.q";
+
+message StaticZeroingSingleDOFProfiledSubsystemTestGoal {
+ double unsafe_goal;
+ .frc971.ProfileParameters profile_params;
+};
+
+message StaticZeroingSingleDOFProfiledSubsystemOutput {
+ double output;
+};
+
+queue_group StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup {
+ implements aos.control_loops.ControlLoop;
+
+ message Status {
+ PotAndAbsoluteEncoderProfiledJointStatus status;
+ };
+
+ message Position {
+ PotAndAbsolutePosition position;
+ };
+
+ queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
+ queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
+ queue Status status;
+ queue Position position;
+};
+
+queue_group StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup {
+ implements aos.control_loops.ControlLoop;
+
+ message Status {
+ AbsoluteEncoderProfiledJointStatus status;
+ };
+
+ message Position {
+ AbsolutePosition position;
+ };
+
+ queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
+ queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
+ queue Status status;
+ queue Position position;
+};
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 75d0b56..41269eb 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -157,9 +157,9 @@
// position is null if the position data is stale, output_enabled is true if
// the output will actually go to the motors, and goal_angle and goal_velocity
// are the goal position and velocities.
- double Update(const ZeroedJoint<kNumZeroSensors>::PositionData *position,
- bool output_enabled,
- double goal_angle, double goal_velocity);
+ double Update(const ::aos::monotonic_clock::time_point monotonic_now,
+ const ZeroedJoint<kNumZeroSensors>::PositionData *position,
+ bool output_enabled, double goal_angle, double goal_velocity);
// True if the code is zeroing.
bool is_zeroing() const { return state_ == ZEROING; }
@@ -270,9 +270,9 @@
// Updates the zeroed joint controller and state machine.
template <int kNumZeroSensors>
double ZeroedJoint<kNumZeroSensors>::Update(
+ const ::aos::monotonic_clock::time_point monotonic_now,
const ZeroedJoint<kNumZeroSensors>::PositionData *position,
- bool output_enabled,
- double goal_angle, double goal_velocity) {
+ bool output_enabled, double goal_angle, double goal_velocity) {
// Uninitialize the bot if too many cycles pass without an encoder.
if (position == NULL) {
LOG(WARNING, "no new pos given\n");
@@ -282,7 +282,7 @@
output_enabled = false;
LOG(WARNING, "err_count is %d so disabling\n", error_count_);
- if (::aos::monotonic_clock::now() > kRezeroTime + last_good_time_) {
+ if (monotonic_now > kRezeroTime + last_good_time_) {
LOG(WARNING, "err_count is %d (or 1st time) so forcing a re-zero\n",
error_count_);
state_ = UNINITIALIZED;
@@ -290,7 +290,7 @@
}
}
if (position != NULL) {
- last_good_time_ = ::aos::monotonic_clock::now();
+ last_good_time_ = monotonic_now;
error_count_ = 0;
}