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/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index e7983f3..969102f 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -81,22 +81,38 @@
class SuperstructureSimulation {
public:
static constexpr double kNoiseScalar = 0.1;
- SuperstructureSimulation()
- : intake_plant_(new IntakePlant(MakeIntakePlant())),
+ SuperstructureSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ superstructure_position_sender_(
+ event_loop_->MakeSender<SuperstructureQueue::Position>(
+ ".y2016.control_loops.superstructure_queue.position")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<SuperstructureQueue::Status>(
+ ".y2016.control_loops.superstructure_queue.status")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<SuperstructureQueue::Output>(
+ ".y2016.control_loops.superstructure_queue.output")),
+ intake_plant_(new IntakePlant(MakeIntakePlant())),
arm_plant_(new ArmPlant(MakeArmPlant())),
pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
pot_encoder_shoulder_(
constants::Values::kShoulderEncoderIndexDifference),
- pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference),
- superstructure_queue_(
- ".y2016.control_loops.superstructure_queue",
- ".y2016.control_loops.superstructure_queue.goal",
- ".y2016.control_loops.superstructure_queue.position",
- ".y2016.control_loops.superstructure_queue.output",
- ".y2016.control_loops.superstructure_queue.status") {
+ pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference) {
InitializeIntakePosition(0.0);
InitializeShoulderPosition(0.0);
InitializeRelativeWristPosition(0.0);
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
}
void InitializeIntakePosition(double start_pos) {
@@ -128,8 +144,8 @@
// Sends a queue message with the position.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<control_loops::SuperstructureQueue::Position>
- position = superstructure_queue_.position.MakeMessage();
+ ::aos::Sender<control_loops::SuperstructureQueue::Position>::Message
+ position = superstructure_position_sender_.MakeMessage();
pot_encoder_intake_.GetSensorValues(&position->intake);
pot_encoder_shoulder_.GetSensorValues(&position->shoulder);
@@ -156,42 +172,45 @@
// Simulates for a single timestep.
void Simulate() {
- EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+ const double begin_shoulder_velocity = shoulder_angular_velocity();
+ const double begin_intake_velocity = intake_angular_velocity();
+ const double begin_wrist_velocity = wrist_angular_velocity();
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
// Feed voltages into physics simulation.
::Eigen::Matrix<double, 1, 1> intake_U;
::Eigen::Matrix<double, 2, 1> arm_U;
- intake_U << superstructure_queue_.output->voltage_intake +
+ intake_U << superstructure_output_fetcher_->voltage_intake +
intake_plant_->voltage_offset();
- arm_U << superstructure_queue_.output->voltage_shoulder +
+ arm_U << superstructure_output_fetcher_->voltage_shoulder +
arm_plant_->shoulder_voltage_offset(),
- superstructure_queue_.output->voltage_wrist +
+ superstructure_output_fetcher_->voltage_wrist +
arm_plant_->wrist_voltage_offset();
// Verify that the correct power limits are being respected depending on
// which mode we are in.
- EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
- if (superstructure_queue_.status->state == Superstructure::RUNNING ||
- superstructure_queue_.status->state ==
+ EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+ if (superstructure_status_fetcher_->state == Superstructure::RUNNING ||
+ superstructure_status_fetcher_->state ==
Superstructure::LANDING_RUNNING) {
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
Superstructure::kOperatingVoltage);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_shoulder),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
Superstructure::kOperatingVoltage);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_wrist),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
Superstructure::kOperatingVoltage);
} else {
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
Superstructure::kZeroingVoltage);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_shoulder),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
Superstructure::kZeroingVoltage);
- CHECK_LE(::std::abs(superstructure_queue_.output->voltage_wrist),
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
Superstructure::kZeroingVoltage);
}
if (arm_plant_->X(0, 0) <=
Superstructure::kShoulderTransitionToLanded + 1e-4) {
- CHECK_GE(superstructure_queue_.output->voltage_shoulder,
+ CHECK_GE(superstructure_output_fetcher_->voltage_shoulder,
Superstructure::kLandingShoulderDownVoltage - 0.00001);
}
@@ -231,127 +250,33 @@
EXPECT_LE(angle_shoulder, constants::Values::kShoulderRange.upper_hard);
EXPECT_GE(angle_wrist, constants::Values::kWristRange.lower_hard);
EXPECT_LE(angle_wrist, constants::Values::kWristRange.upper_hard);
- }
- private:
- ::std::unique_ptr<IntakePlant> intake_plant_;
- ::std::unique_ptr<ArmPlant> arm_plant_;
+ const double loop_time = ::aos::time::DurationInSeconds(dt_);
+ const double shoulder_acceleration =
+ (shoulder_angular_velocity() - begin_shoulder_velocity) / loop_time;
+ const double intake_acceleration =
+ (intake_angular_velocity() - begin_intake_velocity) / loop_time;
+ const double wrist_acceleration =
+ (wrist_angular_velocity() - begin_wrist_velocity) / loop_time;
+ EXPECT_GE(peak_shoulder_acceleration_, shoulder_acceleration);
+ EXPECT_LE(-peak_shoulder_acceleration_, shoulder_acceleration);
+ EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
+ EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
+ EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
+ EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
- PositionSensorSimulator pot_encoder_intake_;
- PositionSensorSimulator pot_encoder_shoulder_;
- PositionSensorSimulator pot_encoder_wrist_;
+ EXPECT_GE(peak_shoulder_velocity_, shoulder_angular_velocity());
+ EXPECT_LE(-peak_shoulder_velocity_, shoulder_angular_velocity());
+ EXPECT_GE(peak_intake_velocity_, intake_angular_velocity());
+ EXPECT_LE(-peak_intake_velocity_, intake_angular_velocity());
+ EXPECT_GE(peak_wrist_velocity_, wrist_angular_velocity());
+ EXPECT_LE(-peak_wrist_velocity_, wrist_angular_velocity());
- SuperstructureQueue superstructure_queue_;
-};
-
-class SuperstructureTest : public ::aos::testing::ControlLoopTest {
- protected:
- SuperstructureTest()
- : superstructure_queue_(
- ".y2016.control_loops.superstructure_queue",
- ".y2016.control_loops.superstructure_queue.goal",
- ".y2016.control_loops.superstructure_queue.position",
- ".y2016.control_loops.superstructure_queue.output",
- ".y2016.control_loops.superstructure_queue.status"),
- superstructure_(&event_loop_),
- superstructure_plant_() {}
-
- void VerifyNearGoal() {
- superstructure_queue_.goal.FetchLatest();
- superstructure_queue_.status.FetchLatest();
-
- EXPECT_TRUE(superstructure_queue_.goal.get() != nullptr);
- EXPECT_TRUE(superstructure_queue_.status.get() != nullptr);
-
- EXPECT_NEAR(superstructure_queue_.goal->angle_intake,
- superstructure_queue_.status->intake.angle, 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->angle_shoulder,
- superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
- superstructure_queue_.status->wrist.angle, 0.001);
-
- EXPECT_NEAR(superstructure_queue_.goal->angle_intake,
- superstructure_plant_.intake_angle(), 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->angle_shoulder,
- superstructure_plant_.shoulder_angle(), 0.001);
- EXPECT_NEAR(superstructure_queue_.goal->angle_wrist,
- superstructure_plant_.wrist_angle(), 0.001);
- }
-
- // Runs one iteration of the whole simulation and checks that separation
- // remains reasonable.
- void RunIteration(bool enabled = true) {
- SendMessages(enabled);
-
- superstructure_plant_.SendPositionMessage();
- superstructure_.Iterate();
- superstructure_plant_.Simulate();
-
- TickTime();
- }
-
- // Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(monotonic_clock::duration run_for, bool enabled = true) {
- 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_shoulder_velocity =
- superstructure_plant_.shoulder_angular_velocity();
- double begin_intake_velocity =
- superstructure_plant_.intake_angular_velocity();
- double begin_wrist_velocity =
- superstructure_plant_.wrist_angular_velocity();
- RunIteration(enabled);
- const double loop_time = ::aos::time::DurationInSeconds(
- monotonic_clock::now() - loop_start_time);
- const double shoulder_acceleration =
- (superstructure_plant_.shoulder_angular_velocity() -
- begin_shoulder_velocity) /
- loop_time;
- const double intake_acceleration =
- (superstructure_plant_.intake_angular_velocity() -
- begin_intake_velocity) /
- loop_time;
- const double wrist_acceleration =
- (superstructure_plant_.wrist_angular_velocity() -
- begin_wrist_velocity) /
- loop_time;
- EXPECT_GE(peak_shoulder_acceleration_, shoulder_acceleration);
- EXPECT_LE(-peak_shoulder_acceleration_, shoulder_acceleration);
- EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
- EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
- EXPECT_GE(peak_wrist_acceleration_, wrist_acceleration);
- EXPECT_LE(-peak_wrist_acceleration_, wrist_acceleration);
-
- EXPECT_GE(peak_shoulder_velocity_,
- superstructure_plant_.shoulder_angular_velocity());
- EXPECT_LE(-peak_shoulder_velocity_,
- superstructure_plant_.shoulder_angular_velocity());
- EXPECT_GE(peak_intake_velocity_,
- superstructure_plant_.intake_angular_velocity());
- EXPECT_LE(-peak_intake_velocity_,
- superstructure_plant_.intake_angular_velocity());
- EXPECT_GE(peak_wrist_velocity_,
- superstructure_plant_.wrist_angular_velocity());
- EXPECT_LE(-peak_wrist_velocity_,
- superstructure_plant_.wrist_angular_velocity());
-
- if (check_for_collisions_) {
- ASSERT_FALSE(collided());
- }
+ if (check_for_collisions_) {
+ ASSERT_FALSE(collided());
}
}
- // Helper function to quickly check if either the estimation detected a
- // collision or if there's a collision using ground-truth plant values.
- bool collided() const {
- return superstructure_.collided() ||
- CollisionAvoidance::collided_with_given_angles(
- superstructure_plant_.shoulder_angle(),
- superstructure_plant_.wrist_angle(),
- superstructure_plant_.intake_angle());
- }
-
// Runs iterations while watching the average acceleration per cycle and
// making sure it doesn't exceed the provided bounds.
void set_peak_intake_acceleration(double value) {
@@ -369,19 +294,33 @@
}
void set_peak_wrist_velocity(double value) { peak_wrist_velocity_ = value; }
- bool check_for_collisions_ = true;
+ void set_check_for_collisions(bool check_for_collisions) {
+ check_for_collisions_ = check_for_collisions;
+ }
- // Create a new instance of the test queue so that it invalidates the queue
- // that it points to. Otherwise, we will have a pointed to
- // shared memory that is no longer valid.
- SuperstructureQueue superstructure_queue_;
-
- ::aos::ShmEventLoop event_loop_;
- // Create a control loop and simulation.
- Superstructure superstructure_;
- SuperstructureSimulation superstructure_plant_;
+ bool collided() const {
+ return CollisionAvoidance::collided_with_given_angles(
+ shoulder_angle(), wrist_angle(), intake_angle());
+ }
private:
+ ::aos::EventLoop *event_loop_;
+ const chrono::nanoseconds dt_;
+
+ bool first_ = true;
+
+ ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+
+ ::std::unique_ptr<IntakePlant> intake_plant_;
+ ::std::unique_ptr<ArmPlant> arm_plant_;
+
+ PositionSensorSimulator pot_encoder_intake_;
+ PositionSensorSimulator pot_encoder_shoulder_;
+ PositionSensorSimulator pot_encoder_wrist_;
+
+ bool check_for_collisions_ = true;
// The acceleration limits to check for while moving for the 3 axes.
double peak_intake_acceleration_ = 1e10;
double peak_shoulder_acceleration_ = 1e10;
@@ -392,42 +331,102 @@
double peak_wrist_velocity_ = 1e10;
};
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ SuperstructureTest()
+ : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ test_event_loop_(MakeEventLoop()),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
+ ".y2016.control_loops.superstructure_queue.goal")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
+ ".y2016.control_loops.superstructure_queue.goal")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
+ ".y2016.control_loops.superstructure_queue.status")),
+ superstructure_event_loop_(MakeEventLoop()),
+ superstructure_(superstructure_event_loop_.get()),
+ superstructure_plant_event_loop_(MakeEventLoop()),
+ superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {}
+
+ void VerifyNearGoal() {
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+
+ EXPECT_TRUE(superstructure_goal_fetcher_.get() != nullptr);
+ EXPECT_TRUE(superstructure_status_fetcher_.get() != nullptr);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
+ superstructure_status_fetcher_->intake.angle, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
+ superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
+ superstructure_status_fetcher_->wrist.angle, 0.001);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
+ superstructure_plant_.intake_angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
+ superstructure_plant_.shoulder_angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
+ superstructure_plant_.wrist_angle(), 0.001);
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+
+ ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+
+ // Create a control loop and simulation.
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
+ Superstructure superstructure_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+ SuperstructureSimulation superstructure_plant_;
+};
+
// Tests that the superstructure does nothing when the goal is zero.
TEST_F(SuperstructureTest, DoesNothing) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0)
- .angle_shoulder(0)
- .angle_wrist(0)
- .max_angular_velocity_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_intake(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0;
+ message->angle_shoulder = 0;
+ message->angle_wrist = 0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- // TODO(phil): Send a goal of some sort.
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
}
// Tests that the loop can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
+ SetEnabled(true);
// Set a reasonable goal.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(M_PI / 4.0)
- .angle_shoulder(1.4)
- .angle_wrist(M_PI / 4.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = M_PI / 4.0;
+ message->angle_shoulder = 1.4;
+ message->angle_wrist = M_PI / 4.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Give it a lot of time to get there.
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
}
@@ -435,109 +434,121 @@
// Tests that the loop doesn't try and go beyond the physical range of the
// mechanisms.
TEST_F(SuperstructureTest, RespectsRange) {
+ SetEnabled(true);
// Set some ridiculous goals to test upper limits.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(M_PI * 10)
- .angle_shoulder(M_PI * 10)
- .angle_wrist(M_PI * 10)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
- RunForTime(chrono::seconds(10));
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = M_PI * 10;
+ message->angle_shoulder = M_PI * 10;
+ message->angle_wrist = M_PI * 10;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_queue_.status->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_queue_.status->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder.angle, 0.001);
EXPECT_NEAR(constants::Values::kWristRange.upper +
constants::Values::kShoulderRange.upper,
- superstructure_queue_.status->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist.angle, 0.001);
// Set some ridiculous goals to test limits.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(M_PI * 10)
- .angle_shoulder(M_PI * 10)
- .angle_wrist(-M_PI * 10.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = M_PI * 10;
+ message->angle_shoulder = M_PI * 10;
+ message->angle_wrist = -M_PI * 10.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_queue_.status->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_queue_.status->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder.angle, 0.001);
EXPECT_NEAR(constants::Values::kWristRange.lower +
constants::Values::kShoulderRange.upper,
- superstructure_queue_.status->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist.angle, 0.001);
// Set some ridiculous goals to test lower limits.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(-M_PI * 10)
- .angle_shoulder(-M_PI * 10)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = -M_PI * 10;
+ message->angle_shoulder = -M_PI * 10;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
- superstructure_queue_.status.FetchLatest();
+ superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
- superstructure_queue_.status->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake.angle, 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.lower,
- superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_queue_.status->wrist.angle, 0.001);
+ superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
}
// Tests that the loop zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(10));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
}
// Tests that the loop zeroes when run for a while without a goal.
TEST_F(SuperstructureTest, ZeroNoGoal) {
- RunForTime(chrono::seconds(5));
+ SetEnabled(true);
+ RunFor(chrono::seconds(5));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
}
// Tests that starting at the lower hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, LowerHardstopStartup) {
+ SetEnabled(true);
// Don't check for collisions for this test.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
@@ -545,40 +556,46 @@
constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.lower);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.upper)
- .angle_shoulder(constants::Values::kShoulderRange.upper)
- .angle_wrist(constants::Values::kWristRange.upper +
- constants::Values::kShoulderRange.upper)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper;
+ message->angle_shoulder = constants::Values::kShoulderRange.upper;
+ message->angle_wrist = constants::Values::kWristRange.upper +
+ constants::Values::kShoulderRange.upper;
+ ASSERT_TRUE(message.Send());
+ }
// We have to wait for it to put the elevator in a safe position as well.
- RunForTime(chrono::seconds(15));
+ RunFor(chrono::seconds(15));
VerifyNearGoal();
}
// Tests that starting at the upper hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, UpperHardstopStartup) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
constants::Values::kShoulderRange.upper);
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(0.0)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
// We have to wait for it to put the superstructure in a safe position as
// well.
- RunForTime(chrono::seconds(20));
+ RunFor(chrono::seconds(20));
VerifyNearGoal();
}
// Tests that resetting WPILib results in a rezero.
TEST_F(SuperstructureTest, ResetTest) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
@@ -586,19 +603,21 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower + 0.3)
- .angle_shoulder(constants::Values::kShoulderRange.upper)
- .angle_wrist(0.0)
- .Send());
- RunForTime(chrono::seconds(15));
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower + 0.3;
+ message->angle_shoulder = constants::Values::kShoulderRange.upper;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(15));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
SimulateSensorReset();
- RunForTime(chrono::milliseconds(100));
+ RunFor(chrono::milliseconds(100));
EXPECT_NE(Superstructure::RUNNING, superstructure_.state());
- RunForTime(chrono::milliseconds(10000));
+ RunFor(chrono::milliseconds(10000));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
}
@@ -606,22 +625,24 @@
// Tests that the internal goals don't change while disabled.
TEST_F(SuperstructureTest, DisabledGoalTest) {
// Don't check for collisions for this test.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower + 0.03)
- .angle_shoulder(constants::Values::kShoulderRange.lower + 0.03)
- .angle_wrist(constants::Values::kWristRange.lower + 0.03)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower + 0.03;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower + 0.03;
+ message->angle_wrist = constants::Values::kWristRange.lower + 0.03;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::milliseconds(100), false);
+ RunFor(chrono::milliseconds(100));
EXPECT_EQ(0.0, superstructure_.intake_.goal(0, 0));
EXPECT_EQ(0.0, superstructure_.arm_.goal(0, 0));
EXPECT_EQ(0.0, superstructure_.arm_.goal(2, 0));
// Now make sure they move correctly
- RunForTime(chrono::milliseconds(4000), true);
+ SetEnabled(true);
+ RunFor(chrono::milliseconds(4000));
EXPECT_NE(0.0, superstructure_.intake_.goal(0, 0));
EXPECT_NE(0.0, superstructure_.arm_.goal(0, 0));
EXPECT_NE(0.0, superstructure_.arm_.goal(2, 0));
@@ -629,6 +650,7 @@
// Tests that disabling while zeroing at any state restarts from beginning
TEST_F(SuperstructureTest, DisabledWhileZeroingHigh) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
@@ -636,17 +658,19 @@
superstructure_plant_.InitializeAbsoluteWristPosition(
constants::Values::kWristRange.upper);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.upper)
- .angle_shoulder(constants::Values::kShoulderRange.upper)
- .angle_wrist(constants::Values::kWristRange.upper)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper;
+ message->angle_shoulder = constants::Values::kShoulderRange.upper;
+ message->angle_wrist = constants::Values::kWristRange.upper;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Expected states to cycle through and check in order.
Superstructure::State kExpectedStateOrder[] = {
@@ -659,7 +683,7 @@
// Cycle through until arm_ and intake_ are initialized in superstructure.cc
while (superstructure_.state() < Superstructure::DISABLED_INITIALIZED) {
- RunIteration(true);
+ RunFor(dt());
}
static const int kNumberOfStates =
@@ -675,7 +699,8 @@
// of 10000 times to ensure a breakout
for (int o = 0;
superstructure_.state() < kExpectedStateOrder[j] && o < 10000; o++) {
- RunIteration(true);
+ RunFor(dt());
+ EXPECT_LT(o, 9999);
}
EXPECT_EQ(kExpectedStateOrder[j], superstructure_.state());
}
@@ -683,34 +708,40 @@
EXPECT_EQ(kExpectedStateOrder[i], superstructure_.state());
// Disable
- RunIteration(false);
+ SetEnabled(false);
+ RunFor(dt());
+ SetEnabled(true);
EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
}
- RunForTime(chrono::seconds(10));
+ SetEnabled(true);
+ RunFor(chrono::seconds(10));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
}
// Tests that disabling while zeroing at any state restarts from beginning
TEST_F(SuperstructureTest, DisabledWhileZeroingLow) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(constants::Values::kWristRange.lower)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.lower;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = constants::Values::kWristRange.lower;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Expected states to cycle through and check in order.
Superstructure::State kExpectedStateOrder[] = {
@@ -723,7 +754,7 @@
// Cycle through until arm_ and intake_ are initialized in superstructure.cc
while (superstructure_.state() < Superstructure::DISABLED_INITIALIZED) {
- RunIteration(true);
+ RunFor(dt());
}
static const int kNumberOfStates =
@@ -739,7 +770,8 @@
// of 10000 times to ensure a breakout
for (int o = 0;
superstructure_.state() < kExpectedStateOrder[j] && o < 10000; o++) {
- RunIteration(true);
+ RunFor(dt());
+ EXPECT_LT(o, 9999);
}
EXPECT_EQ(kExpectedStateOrder[j], superstructure_.state());
}
@@ -747,12 +779,15 @@
EXPECT_EQ(kExpectedStateOrder[i], superstructure_.state());
// Disable
- RunIteration(false);
+ SetEnabled(false);
+ RunFor(dt());
+ SetEnabled(true);
EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
}
- RunForTime(chrono::seconds(10));
+ SetEnabled(true);
+ RunFor(chrono::seconds(10));
EXPECT_EQ(Superstructure::LANDING_RUNNING, superstructure_.state());
}
@@ -766,8 +801,9 @@
// Tests that the integrators works.
TEST_F(SuperstructureTest, IntegratorTest) {
+ SetEnabled(true);
// Don't check for collisions for this test.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
@@ -775,13 +811,15 @@
constants::Values::kShoulderRange.lower);
superstructure_plant_.InitializeRelativeWristPosition(0.0);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
- .Send();
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 0.0;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(8));
+ RunFor(chrono::seconds(8));
VerifyNearGoal();
}
@@ -795,168 +833,185 @@
constants::Values::kShoulderEncoderIndexDifference * 10 - 0.001);
superstructure_plant_.InitializeRelativeWristPosition(-0.001);
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(constants::Values::kShoulderEncoderIndexDifference * 10)
- .angle_wrist(0.0)
- .Send();
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ constants::Values::kShoulderEncoderIndexDifference * 10;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
// Run disabled for 2 seconds
- RunForTime(chrono::seconds(2), false);
+ RunFor(chrono::seconds(2));
EXPECT_EQ(Superstructure::DISABLED_INITIALIZED, superstructure_.state());
superstructure_plant_.set_power_error(1.0, 1.5, 1.0);
- RunForTime(chrono::seconds(1), false);
+ RunFor(chrono::seconds(1));
EXPECT_EQ(Superstructure::SLOW_RUNNING, superstructure_.state());
- RunForTime(chrono::seconds(2), true);
+ SetEnabled(true);
+ RunFor(chrono::seconds(2));
VerifyNearGoal();
}
// Tests that the zeroing errors in the arm are caught
TEST_F(SuperstructureTest, ArmZeroingErrorTest) {
- RunIteration();
+ SetEnabled(true);
+ RunFor(2 * dt());
EXPECT_NE(Superstructure::ESTOP, superstructure_.state());
superstructure_.arm_.TriggerEstimatorError();
- RunIteration();
+ RunFor(2 * dt());
EXPECT_EQ(Superstructure::ESTOP, superstructure_.state());
}
// Tests that the zeroing errors in the intake are caught
TEST_F(SuperstructureTest, IntakeZeroingErrorTest) {
- RunIteration();
+ SetEnabled(true);
+ RunFor(2 * dt());
EXPECT_NE(Superstructure::ESTOP, superstructure_.state());
superstructure_.intake_.TriggerEstimatorError();
- RunIteration();
+ RunFor(2 * dt());
EXPECT_EQ(Superstructure::ESTOP, superstructure_.state());
}
// Tests that the loop respects shoulder acceleration limits while moving.
TEST_F(SuperstructureTest, ShoulderAccelerationLimitTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.5)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(1)
- .max_angular_acceleration_intake(1)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(1)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(1)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.5;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 1;
+ message->max_angular_acceleration_intake = 1;
+ message->max_angular_velocity_shoulder = 1;
+ message->max_angular_acceleration_shoulder = 1;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 1;
+ ASSERT_TRUE(message.Send());
+ }
// TODO(austin): The profile isn't feasible, so when we try to track it, we
// have trouble going from the acceleration step to the constant velocity
// step. We end up under and then overshooting.
- set_peak_intake_acceleration(1.10);
- set_peak_shoulder_acceleration(1.20);
- set_peak_wrist_acceleration(1.10);
- RunForTime(chrono::seconds(6));
+ superstructure_plant_.set_peak_intake_acceleration(1.10);
+ superstructure_plant_.set_peak_shoulder_acceleration(1.20);
+ superstructure_plant_.set_peak_wrist_acceleration(1.10);
+ RunFor(chrono::seconds(6));
VerifyNearGoal();
}
// Tests that the loop respects intake acceleration limits while moving.
TEST_F(SuperstructureTest, IntakeAccelerationLimitTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.5)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(1)
- .max_angular_acceleration_intake(1)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(1)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(1)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.5;
+ message->angle_shoulder = 1.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 1;
+ message->max_angular_acceleration_intake = 1;
+ message->max_angular_velocity_shoulder = 1;
+ message->max_angular_acceleration_shoulder = 1;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 1;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_acceleration(1.20);
- set_peak_shoulder_acceleration(1.20);
- set_peak_wrist_acceleration(1.20);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_acceleration(1.20);
+ superstructure_plant_.set_peak_shoulder_acceleration(1.20);
+ superstructure_plant_.set_peak_wrist_acceleration(1.20);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
// Tests that the loop respects wrist acceleration limits while moving.
TEST_F(SuperstructureTest, WristAccelerationLimitTest) {
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
- .angle_wrist(0.5)
- .max_angular_velocity_intake(1)
- .max_angular_acceleration_intake(1)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(1)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(1)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
+ message->angle_wrist = 0.5;
+ message->max_angular_velocity_intake = 1;
+ message->max_angular_acceleration_intake = 1;
+ message->max_angular_velocity_shoulder = 1;
+ message->max_angular_acceleration_shoulder = 1;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 1;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_acceleration(1.05);
- set_peak_shoulder_acceleration(1.05);
- set_peak_wrist_acceleration(1.05);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_acceleration(1.05);
+ superstructure_plant_.set_peak_shoulder_acceleration(1.05);
+ superstructure_plant_.set_peak_wrist_acceleration(1.05);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
@@ -964,43 +1019,46 @@
// Tests that the loop respects intake handles saturation while accelerating
// correctly.
TEST_F(SuperstructureTest, SaturatedIntakeProfileTest) {
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.5)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(4.5)
- .max_angular_acceleration_intake(800)
- .max_angular_velocity_shoulder(1)
- .max_angular_acceleration_shoulder(100)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(100)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.5;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 4.5;
+ message->max_angular_acceleration_intake = 800;
+ message->max_angular_velocity_shoulder = 1;
+ message->max_angular_acceleration_shoulder = 100;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 100;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_velocity(4.65);
- set_peak_shoulder_velocity(1.00);
- set_peak_wrist_velocity(1.00);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_velocity(4.65);
+ superstructure_plant_.set_peak_shoulder_velocity(1.00);
+ superstructure_plant_.set_peak_wrist_velocity(1.00);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
@@ -1008,39 +1066,44 @@
// Tests that the loop respects shoulder handles saturation while accelerating
// correctly.
TEST_F(SuperstructureTest, SaturatedShoulderProfileTest) {
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(1.9)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(1.0)
- .max_angular_acceleration_intake(1.0)
- .max_angular_velocity_shoulder(5.0)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(1)
- .max_angular_acceleration_wrist(100)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 1.9;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 1.0;
+ message->max_angular_acceleration_intake = 1.0;
+ message->max_angular_velocity_shoulder = 5.0;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 1;
+ message->max_angular_acceleration_wrist = 100;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_velocity(1.0);
- set_peak_shoulder_velocity(5.5);
- set_peak_wrist_velocity(1.0);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_velocity(1.0);
+ superstructure_plant_.set_peak_shoulder_velocity(5.5);
+ superstructure_plant_.set_peak_wrist_velocity(1.0);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
@@ -1048,45 +1111,46 @@
// Tests that the loop respects wrist handles saturation while accelerating
// correctly.
TEST_F(SuperstructureTest, SaturatedWristProfileTest) {
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ SetEnabled(true);
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
VerifyNearGoal();
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
- .angle_wrist(1.3)
- .max_angular_velocity_intake(1.0)
- .max_angular_acceleration_intake(1.0)
- .max_angular_velocity_shoulder(1.0)
- .max_angular_acceleration_shoulder(1.0)
- .max_angular_velocity_wrist(10.0)
- .max_angular_acceleration_wrist(160.0)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder =
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
+ message->angle_wrist = 1.3;
+ message->max_angular_velocity_intake = 1.0;
+ message->max_angular_acceleration_intake = 1.0;
+ message->max_angular_velocity_shoulder = 1.0;
+ message->max_angular_acceleration_shoulder = 1.0;
+ message->max_angular_velocity_wrist = 10.0;
+ message->max_angular_acceleration_wrist = 160.0;
+ ASSERT_TRUE(message.Send());
+ }
- set_peak_intake_velocity(1.0);
- set_peak_shoulder_velocity(1.0);
- set_peak_wrist_velocity(10.2);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_intake_velocity(1.0);
+ superstructure_plant_.set_peak_shoulder_velocity(1.0);
+ superstructure_plant_.set_peak_wrist_velocity(10.2);
+ RunFor(chrono::seconds(4));
VerifyNearGoal();
}
@@ -1094,243 +1158,275 @@
// Make sure that the intake moves out of the way when the arm wants to move
// into a shooting position.
TEST_F(SuperstructureTest, AvoidCollisionWhenMovingArmFromStart) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(0.0);
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.upper) // stowed
- .angle_shoulder(constants::Values::kShoulderRange.lower) // Down
- .angle_wrist(0.0) // Stowed
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
+ message->angle_shoulder = constants::Values::kShoulderRange.lower; // Down
+ message->angle_wrist = 0.0; // Stowed
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(15));
+ RunFor(chrono::seconds(15));
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.upper) // stowed
- .angle_shoulder(M_PI / 4.0) // in the collision area
- .angle_wrist(M_PI / 2.0) // down
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
+ message->angle_shoulder = M_PI / 4.0; // in the collision area
+ message->angle_wrist = M_PI / 2.0; // down
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be out of the way despite being told to move to stowing.
- EXPECT_LT(superstructure_queue_.status->intake.angle, M_PI);
- EXPECT_LT(superstructure_queue_.status->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake.angle, M_PI);
+ EXPECT_LT(superstructure_status_fetcher_->intake.angle,
constants::Values::kIntakeRange.upper);
- EXPECT_LT(superstructure_queue_.status->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake.angle,
CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
// The arm should have reached its goal.
- EXPECT_NEAR(M_PI / 4.0, superstructure_queue_.status->shoulder.angle, 0.001);
+ EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
// The wrist should be forced into a stowing position.
// Since the intake is kicked out, we can be within
// kMaxWristAngleForMovingByIntake
- EXPECT_NEAR(0, superstructure_queue_.status->wrist.angle,
+ EXPECT_NEAR(0, superstructure_status_fetcher_->wrist.angle,
CollisionAvoidance::kMaxWristAngleForMovingByIntake + 0.001);
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.upper) // stowed
- .angle_shoulder(M_PI / 2.0) // in the collision area
- .angle_wrist(M_PI) // forward
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
+ message->angle_shoulder = M_PI / 2.0; // in the collision area
+ message->angle_wrist = M_PI; // forward
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(5));
+ RunFor(chrono::seconds(5));
VerifyNearGoal();
}
// Make sure that the shooter holds itself level when the arm comes down
// into a stowing/intaking position.
TEST_F(SuperstructureTest, AvoidCollisionWhenStowingArm) {
+ SetEnabled(true);
superstructure_plant_.InitializeIntakePosition(0.0); // forward
superstructure_plant_.InitializeShoulderPosition(M_PI / 2.0); // up
superstructure_plant_.InitializeAbsoluteWristPosition(M_PI); // forward
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(M_PI) // intentionally asking for forward
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 0.0;
+ message->angle_wrist = M_PI; // intentionally asking for forward
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(15));
+ RunFor(chrono::seconds(15));
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be in intaking position, as asked.
- EXPECT_NEAR(0.0, superstructure_queue_.status->intake.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake.angle, 0.001);
// The shoulder and wrist should both be at zero degrees (i.e.
// stowed/intaking position).
- EXPECT_NEAR(0.0, superstructure_queue_.status->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_queue_.status->wrist.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
}
// Make sure that we can properly detect a collision.
TEST_F(SuperstructureTest, DetectAndFixCollisionBetweenArmAndIntake) {
+ SetEnabled(true);
// Zero & go straight up with the shoulder.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(M_PI * 0.5)
- .angle_wrist(0.0)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = M_PI * 0.5;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
VerifyNearGoal();
// Since we're explicitly checking for collisions, we don't want to fail the
// test because of collisions.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
// Move shoulder down until collided by applying a voltage offset while
// disabled.
superstructure_plant_.set_power_error(0.0, -1.0, 0.0);
- while (!collided()) {
- RunIteration(false);
+ SetEnabled(false);
+ while (!superstructure_plant_.collided()) {
+ RunFor(dt());
}
- RunForTime(chrono::milliseconds(500), false); // Move a bit further down.
+ RunFor(chrono::milliseconds(500)); // Move a bit further down.
- ASSERT_TRUE(collided());
+ ASSERT_TRUE(superstructure_plant_.collided());
EXPECT_EQ(Superstructure::SLOW_RUNNING, superstructure_.state());
superstructure_plant_.set_power_error(0.0, 0.0, 0.0);
// Make sure that the collision avoidance will properly move the limbs out of
// the collision area.
- RunForTime(chrono::seconds(10));
- ASSERT_FALSE(collided());
+ SetEnabled(true);
+ RunFor(chrono::seconds(10));
+ ASSERT_FALSE(superstructure_plant_.collided());
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
}
// Make sure that we can properly detect a collision.
TEST_F(SuperstructureTest, DetectAndFixShoulderInDrivebase) {
+ SetEnabled(true);
// Zero & go straight up with the shoulder.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(0.0)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
VerifyNearGoal();
// Since we're explicitly checking for collisions, we don't want to fail the
// test because of collisions.
- check_for_collisions_ = false;
+ superstructure_plant_.set_check_for_collisions(false);
// Move wrist up until on top of the bellypan
superstructure_plant_.set_power_error(0.0, 0.0, -1.0);
+ SetEnabled(false);
while (superstructure_plant_.wrist_angle() > -0.2) {
- RunIteration(false);
+ RunFor(dt());
}
- ASSERT_TRUE(collided());
+ ASSERT_TRUE(superstructure_plant_.collided());
EXPECT_EQ(Superstructure::LANDING_SLOW_RUNNING, superstructure_.state());
// Make sure that the collision avoidance will properly move the limbs out of
// the collision area.
superstructure_plant_.set_power_error(0.0, 0.0, 0.0);
- RunForTime(chrono::seconds(3));
- ASSERT_FALSE(collided());
+ SetEnabled(true);
+ RunFor(chrono::seconds(3));
+ ASSERT_FALSE(superstructure_plant_.collided());
EXPECT_EQ(Superstructure::LANDING_RUNNING, superstructure_.state());
}
// Make sure that the landing voltage limit works.
TEST_F(SuperstructureTest, LandingDownVoltageLimit) {
+ SetEnabled(true);
+
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(0.0);
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(constants::Values::kShoulderRange.lower)
- .angle_wrist(0.0) // intentionally asking for forward
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = constants::Values::kShoulderRange.lower;
+ message->angle_wrist = 0.0; // intentionally asking for forward
+ ASSERT_TRUE(message.Send());
+ }
- RunForTime(chrono::seconds(6));
+ RunFor(chrono::seconds(6));
VerifyNearGoal();
// If we are near the bottom of the range, we won't have enough power to
// compensate for the offset. This means that we fail if we get to the goal.
superstructure_plant_.set_power_error(
0.0, -Superstructure::kLandingShoulderDownVoltage, 0.0);
- RunForTime(chrono::seconds(2));
+ RunFor(chrono::seconds(2));
superstructure_plant_.set_power_error(
0.0, -2.0 * Superstructure::kLandingShoulderDownVoltage, 0.0);
- RunForTime(chrono::seconds(2));
+ RunFor(chrono::seconds(2));
+ superstructure_goal_fetcher_.Fetch();
EXPECT_LE(constants::Values::kShoulderRange.lower,
- superstructure_queue_.goal->angle_shoulder);
+ superstructure_goal_fetcher_->angle_shoulder);
}
// Make sure that we land slowly.
TEST_F(SuperstructureTest, LandSlowly) {
+ SetEnabled(true);
// Zero & go to initial position.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(M_PI * 0.25)
- .angle_wrist(0.0)
- .Send());
- RunForTime(chrono::seconds(8));
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = M_PI * 0.25;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(8));
// Tell it to land in the bellypan as fast as possible.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 0.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Wait until we hit the transition point.
do {
- RunIteration();
- superstructure_queue_.status.FetchLatest();
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
} while (superstructure_plant_.shoulder_angle() >
Superstructure::kShoulderTransitionToLanded);
- set_peak_shoulder_velocity(0.55);
- RunForTime(chrono::seconds(4));
+ superstructure_plant_.set_peak_shoulder_velocity(0.55);
+ RunFor(chrono::seconds(4));
}
// Make sure that we quickly take off from a land.
TEST_F(SuperstructureTest, TakeOffQuickly) {
+ SetEnabled(true);
// Zero & go to initial position.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(0.0)
- .angle_wrist(0.0)
- .Send());
- RunForTime(chrono::seconds(8));
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = 0.0;
+ message->angle_wrist = 0.0;
+ ASSERT_TRUE(message.Send());
+ }
+ RunFor(chrono::seconds(8));
// Tell it to take off as fast as possible.
- ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(0.0)
- .angle_shoulder(M_PI / 2.0)
- .angle_wrist(0.0)
- .max_angular_velocity_intake(20)
- .max_angular_acceleration_intake(20)
- .max_angular_velocity_shoulder(20)
- .max_angular_acceleration_shoulder(20)
- .max_angular_velocity_wrist(20)
- .max_angular_acceleration_wrist(20)
- .Send());
+ {
+ auto message = superstructure_goal_sender_.MakeMessage();
+ message->angle_intake = 0.0;
+ message->angle_shoulder = M_PI / 2.0;
+ message->angle_wrist = 0.0;
+ message->max_angular_velocity_intake = 20;
+ message->max_angular_acceleration_intake = 20;
+ message->max_angular_velocity_shoulder = 20;
+ message->max_angular_acceleration_shoulder = 20;
+ message->max_angular_velocity_wrist = 20;
+ message->max_angular_acceleration_wrist = 20;
+ ASSERT_TRUE(message.Send());
+ }
// Wait until we hit the transition point.
do {
- RunIteration();
- superstructure_queue_.status.FetchLatest();
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
} while (superstructure_plant_.shoulder_angle() <
Superstructure::kShoulderTransitionToLanded);
diff --git a/y2016/control_loops/superstructure/superstructure_main.cc b/y2016/control_loops/superstructure/superstructure_main.cc
index 435724b..00fa7dd 100644
--- a/y2016/control_loops/superstructure/superstructure_main.cc
+++ b/y2016/control_loops/superstructure/superstructure_main.cc
@@ -4,11 +4,14 @@
#include "aos/init.h"
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2016::control_loops::superstructure::Superstructure superstructure(
&event_loop);
- superstructure.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}