generalized faking queue messages for test control loops and used it
diff --git a/aos/common/controls/control_loop_test.cc b/aos/common/controls/control_loop_test.cc
new file mode 100644
index 0000000..6f4e784
--- /dev/null
+++ b/aos/common/controls/control_loop_test.cc
@@ -0,0 +1,57 @@
+#include "aos/common/controls/control_loop_test.h"
+
+#include "aos/common/messages/robot_state.q.h"
+#include "aos/common/controls/sensor_generation.q.h"
+#include "aos/common/controls/output_check.q.h"
+#include "aos/common/time.h"
+
+namespace aos {
+namespace testing {
+
+ControlLoopTest::ControlLoopTest() {
+ ::aos::robot_state.Clear();
+ ::aos::controls::sensor_generation.Clear();
+ ::aos::controls::output_check_received.Clear();
+
+ ::aos::controls::sensor_generation.MakeWithBuilder()
+ .reader_pid(254)
+ .cape_resets(5)
+ .Send();
+ ::aos::time::Time::EnableMockTime(::aos::time::Time::InSeconds(0.0));
+
+ SimulateTimestep(false);
+}
+
+ControlLoopTest::~ControlLoopTest() {
+ ::aos::robot_state.Clear();
+ ::aos::controls::sensor_generation.Clear();
+ ::aos::controls::output_check_received.Clear();
+
+ ::aos::time::Time::DisableMockTime();
+}
+
+void ControlLoopTest::SimulateTimestep(bool enabled) {
+ if (sent_robot_state_last_time_) {
+ sent_robot_state_last_time_ = false;
+ } else {
+ ::aos::robot_state.MakeWithBuilder()
+ .enabled(enabled)
+ .autonomous(false)
+ .fake(true)
+ .team_id(971)
+ .Send();
+ sent_robot_state_last_time_ = true;
+ }
+ if (enabled) {
+ // TODO(brians): Actually make this realistic once we figure out what that
+ // means.
+ ::aos::controls::output_check_received.MakeWithBuilder()
+ .pwm_value(0)
+ .pulse_length(0)
+ .Send();
+ }
+ ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/common/controls/control_loop_test.h b/aos/common/controls/control_loop_test.h
new file mode 100644
index 0000000..14be01c
--- /dev/null
+++ b/aos/common/controls/control_loop_test.h
@@ -0,0 +1,34 @@
+#ifndef AOS_COMMON_CONTROLS_CONTROL_LOOP_TEST_H_
+#define AOS_COMMON_CONTROLS_CONTROL_LOOP_TEST_H_
+
+#include "gtest/gtest.h"
+
+#include "aos/common/queue_testutils.h"
+
+namespace aos {
+namespace testing {
+
+// Handles setting up the environment that all control loops need to actually
+// run.
+// This includes sending the queue messages and Clear()ing the queues when
+// appropriate.
+// It also includes dealing with ::aos::time.
+class ControlLoopTest : public ::testing::Test {
+ public:
+ ControlLoopTest();
+
+ virtual ~ControlLoopTest();
+
+ // Simulates everything that happens during 1 time step.
+ void SimulateTimestep(bool enabled);
+
+ private:
+ bool sent_robot_state_last_time_ = false;
+
+ ::aos::common::testing::GlobalCoreInstance my_core;
+};
+
+} // namespace testing
+} // namespace aos
+
+#endif // AOS_COMMON_CONTROLS_CONTROL_LOOP_TEST_H_
diff --git a/aos/common/controls/controls.gyp b/aos/common/controls/controls.gyp
index 0945831..15be23c 100644
--- a/aos/common/controls/controls.gyp
+++ b/aos/common/controls/controls.gyp
@@ -1,6 +1,25 @@
{
'targets': [
{
+ 'target_name': 'control_loop_test',
+ 'type': 'static_library',
+ 'sources': [
+ 'control_loop_test.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
+ 'sensor_generation',
+ 'output_check',
+ '<(EXTERNALS):gtest',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ ],
+ },
+ {
'target_name': 'polytope',
'type': 'static_library',
'sources': [
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 2575adf..58a2d2e 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -52,9 +52,8 @@
'<(EXTERNALS):gtest',
'claw_loop',
'claw_lib',
- '<(AOS)/common/common.gyp:queue_testutils',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(AOS)/common/common.gyp:queues',
+ '<(AOS)/common/controls/controls.gyp:control_loop_test',
],
},
{
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index a385b93..79f9522 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -4,7 +4,8 @@
#include "aos/common/network/team_number.h"
#include "aos/common/queue.h"
-#include "aos/common/queue_testutils.h"
+#include "aos/common/controls/control_loop_test.h"
+
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw.h"
#include "frc971/constants.h"
@@ -13,8 +14,6 @@
#include "gtest/gtest.h"
-using ::aos::time::Time;
-
namespace frc971 {
namespace control_loops {
namespace testing {
@@ -231,7 +230,6 @@
v.claw.claw_max_separation);
EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
v.claw.claw_min_separation);
- ::aos::time::Time::IncrementMockTime(::aos::time::Time::InMS(10.0));
}
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -249,10 +247,8 @@
};
-class ClawTest : public ::testing::Test {
+class ClawTest : public ::aos::testing::ControlLoopTest {
protected:
- ::aos::common::testing::GlobalCoreInstance my_core;
-
// 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.
@@ -275,25 +271,6 @@
claw_motor_(&claw_queue_group),
claw_motor_plant_(0.4, 0.2),
min_separation_(constants::GetValues().claw.claw_min_separation) {
- // Flush the robot state queue so we can use clean shared memory for this
- // test.
- ::aos::robot_state.Clear();
- SendDSPacket(true);
- ::aos::controls::sensor_generation.Clear();
- ::aos::controls::sensor_generation.MakeWithBuilder()
- .reader_pid(254)
- .cape_resets(5)
- .Send();
- ::aos::time::Time::EnableMockTime(::aos::time::Time::InSeconds(0.0));
- }
-
- void SendDSPacket(bool enabled) {
- ::aos::robot_state.MakeWithBuilder()
- .enabled(enabled)
- .autonomous(false)
- .team_id(971)
- .Send();
- ::aos::robot_state.FetchLatest();
}
void VerifyNearGoal() {
@@ -306,13 +283,6 @@
EXPECT_NEAR(claw_queue_group.goal->separation_angle, separation, 1e-4);
EXPECT_LE(min_separation_, separation);
}
-
-
- virtual ~ClawTest() {
- ::aos::robot_state.Clear();
- ::aos::controls::sensor_generation.Clear();
- ::aos::time::Time::DisableMockTime();
- }
};
TEST_F(ClawTest, HandlesNAN) {
@@ -324,7 +294,7 @@
claw_motor_plant_.SendPositionMessage();
claw_motor_.Iterate();
claw_motor_plant_.Simulate();
- SendDSPacket(true);
+ SimulateTimestep(true);
}
}
@@ -338,7 +308,7 @@
claw_motor_plant_.SendPositionMessage();
claw_motor_.Iterate();
claw_motor_plant_.Simulate();
- SendDSPacket(true);
+ SimulateTimestep(true);
}
VerifyNearGoal();
}
@@ -435,7 +405,7 @@
claw_motor_.Iterate();
claw_motor_plant_.Simulate();
- SendDSPacket(true);
+ SimulateTimestep(true);
}
VerifyNearGoal();
}
@@ -455,7 +425,7 @@
claw_motor_.Iterate();
claw_motor_plant_.Simulate();
- SendDSPacket(true);
+ SimulateTimestep(true);
}
VerifyNearGoal();
}
@@ -516,7 +486,7 @@
claw_motor_.Iterate();
claw_motor_plant_.Simulate();
- SendDSPacket(true);
+ SimulateTimestep(true);
}
VerifyNearGoal();
}
@@ -532,7 +502,7 @@
claw_motor_plant_.SendPositionMessage();
// After 0.5 seconds, disable the robot.
if (i > 50 && i < 200) {
- SendDSPacket(false);
+ SimulateTimestep(false);
if (i > 100) {
// Give the loop a couple cycled to get the message and then verify that
// it is in the correct state.
@@ -543,7 +513,7 @@
EXPECT_EQ(0, claw_queue_group.output->bottom_claw_voltage);
}
} else {
- SendDSPacket(true);
+ SimulateTimestep(true);
}
if (i == 202) {
// Verify that we are zeroing after the bot gets enabled again.
@@ -610,7 +580,7 @@
claw_motor_.Iterate();
claw_motor_plant_.Simulate();
- SendDSPacket(true);
+ SimulateTimestep(true);
}
EXPECT_TRUE(kicked);
EXPECT_TRUE(measured);