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/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index d87c638..d1ef2a5 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -219,6 +219,7 @@
const ::y2014::control_loops::ShooterQueue::Position *position,
::y2014::control_loops::ShooterQueue::Output *output,
::y2014::control_loops::ShooterQueue::Status *status) {
+ const monotonic_clock::time_point monotonic_now = event_loop()->monotonic_now();
if (goal && ::std::isnan(goal->shot_power)) {
state_ = STATE_ESTOP;
LOG(ERROR, "Estopping because got a shot power of NAN.\n");
@@ -332,11 +333,11 @@
// The plunger is back and we are latched. We most likely got here
// from Initialize, in which case we want to 'load' again anyways to
// zero.
- Load();
+ Load(monotonic_now);
latch_piston_ = true;
} else {
// Off the sensor, start loading.
- Load();
+ Load(monotonic_now);
latch_piston_ = false;
}
}
@@ -361,7 +362,7 @@
if (position) {
if (!position->pusher_distal.current &&
!position->pusher_proximal.current) {
- Load();
+ Load(monotonic_now);
}
latch_piston_ = position->plunger;
}
@@ -371,7 +372,7 @@
case STATE_LOAD:
// If we are disabled right now, reset the timer.
if (disabled) {
- Load();
+ Load(monotonic_now);
// Latch defaults to true when disabled. Leave it latched until we have
// useful sensor data.
latch_piston_ = true;
@@ -406,17 +407,17 @@
// We are at the goal, but not latched.
state_ = STATE_LOADING_PROBLEM;
loading_problem_end_time_ =
- monotonic_clock::now() + kLoadProblemEndTimeout;
+ monotonic_now + kLoadProblemEndTimeout;
}
}
- if (load_timeout_ < monotonic_clock::now()) {
+ if (load_timeout_ < monotonic_now) {
if (position) {
// If none of the sensors is triggered, estop.
// Otherwise, trigger anyways if it has been 0.5 seconds more.
if (!(position->pusher_distal.current ||
position->pusher_proximal.current) ||
(load_timeout_ + chrono::milliseconds(500) <
- monotonic_clock::now())) {
+ monotonic_now)) {
state_ = STATE_ESTOP;
LOG(ERROR, "Estopping because took too long to load.\n");
}
@@ -431,9 +432,9 @@
}
// We got to the goal, but the latch hasn't registered as down. It might
// be stuck, or on it's way but not there yet.
- if (monotonic_clock::now() > loading_problem_end_time_) {
+ if (monotonic_now > loading_problem_end_time_) {
// Timeout by unloading.
- Unload();
+ Unload(monotonic_now);
} else if (position && position->plunger && position->latch) {
// If both trigger, we are latched.
state_ = STATE_PREPARE_SHOT;
@@ -471,21 +472,21 @@
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
- shooter_brake_set_time_ = monotonic_clock::now() + kShooterBrakeSetTime;
+ shooter_brake_set_time_ = monotonic_now + kShooterBrakeSetTime;
state_ = STATE_READY;
} else {
latch_piston_ = true;
brake_piston_ = false;
}
if (goal && goal->unload_requested) {
- Unload();
+ Unload(monotonic_now);
}
break;
case STATE_READY:
LOG(DEBUG, "In ready\n");
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
- if (monotonic_clock::now() > shooter_brake_set_time_) {
+ if (monotonic_now > shooter_brake_set_time_) {
status->ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
@@ -494,7 +495,7 @@
if (goal && goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- shot_end_time_ = monotonic_clock::now() + kShotEndTimeout;
+ shot_end_time_ = monotonic_now + kShotEndTimeout;
firing_starting_position_ = shooter_.absolute_position();
state_ = STATE_FIRE;
}
@@ -519,7 +520,7 @@
brake_piston_ = true;
if (goal && goal->unload_requested) {
- Unload();
+ Unload(monotonic_now);
}
break;
@@ -529,7 +530,7 @@
if (position->plunger) {
// If disabled and the plunger is still back there, reset the
// timeout.
- shot_end_time_ = monotonic_clock::now() + kShotEndTimeout;
+ shot_end_time_ = monotonic_now + kShotEndTimeout;
}
}
}
@@ -548,7 +549,7 @@
if (((::std::abs(firing_starting_position_ -
shooter_.absolute_position()) > 0.0005 &&
cycles_not_moved_ > 6) ||
- monotonic_clock::now() > shot_end_time_) &&
+ monotonic_now > shot_end_time_) &&
robot_state().voltage_battery > 10.5) {
state_ = STATE_REQUEST_LOAD;
++shot_count_;
@@ -558,7 +559,7 @@
break;
case STATE_UNLOAD:
// Reset the timeouts.
- if (disabled) Unload();
+ if (disabled) Unload(monotonic_now);
// If it is latched and the plunger is back, move the pusher back to catch
// the plunger.
@@ -588,10 +589,10 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
latch_piston_ = false;
state_ = STATE_UNLOAD_MOVE;
- unload_timeout_ = monotonic_clock::now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_now + kUnloadTimeout;
}
- if (monotonic_clock::now() > unload_timeout_) {
+ if (monotonic_now > unload_timeout_) {
// We have been stuck trying to unload for way too long, give up and
// turn everything off.
state_ = STATE_ESTOP;
@@ -602,7 +603,7 @@
break;
case STATE_UNLOAD_MOVE: {
if (disabled) {
- unload_timeout_ = monotonic_clock::now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_now + kUnloadTimeout;
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index a9a255b..01096a0 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -175,14 +175,14 @@
friend class testing::ShooterTest_RezeroWhileUnloading_Test;
// Enter state STATE_UNLOAD
- void Unload() {
+ void Unload(::aos::monotonic_clock::time_point monotonic_now) {
state_ = STATE_UNLOAD;
- unload_timeout_ = ::aos::monotonic_clock::now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_now + kUnloadTimeout;
}
// Enter state STATE_LOAD
- void Load() {
+ void Load(::aos::monotonic_clock::time_point monotonic_now) {
state_ = STATE_LOAD;
- load_timeout_ = ::aos::monotonic_clock::now() + kLoadTimeout;
+ load_timeout_ = monotonic_now + kLoadTimeout;
}
::y2014::control_loops::ShooterQueue::Position last_position_;
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index e88a33e..b6883ad 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -28,19 +28,32 @@
class ShooterSimulation {
public:
// Constructs a motor simulation.
- ShooterSimulation(double initial_position)
- : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
+ ShooterSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt,
+ double initial_position)
+ : event_loop_(event_loop),
+ shooter_position_sender_(
+ event_loop_->MakeSender<ShooterQueue::Position>(
+ ".y2014.control_loops.shooter_queue.position")),
+ shooter_output_fetcher_(event_loop_->MakeFetcher<ShooterQueue::Output>(
+ ".y2014.control_loops.shooter_queue.output")),
+ shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
latch_piston_state_(false),
latch_delay_count_(0),
plunger_latched_(false),
brake_piston_state_(true),
- brake_delay_count_(0),
- shooter_queue_(".y2014.control_loops.shooter_queue",
- ".y2014.control_loops.shooter_queue.goal",
- ".y2014.control_loops.shooter_queue.position",
- ".y2014.control_loops.shooter_queue.output",
- ".y2014.control_loops.shooter_queue.status") {
+ brake_delay_count_(0) {
Reinitialize(initial_position);
+
+ event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ Simulate();
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
}
// The difference between the position with 0 at the back, and the position
@@ -133,25 +146,24 @@
}
}
- void SendPositionMessage() {
- // the first bool is false
- SendPositionMessage(false, false, false, false);
- }
+ void set_use_passed(bool use_passed) { use_passed_ = use_passed; }
+ void set_plunger_in(bool plunger_in) { plunger_in_ = plunger_in; }
+ void set_latch_in(bool latch_in) { latch_in_ = latch_in; }
+ void set_brake_in(bool brake_in) { brake_in_ = brake_in; }
// Sends out the position queue messages.
- // if the first bool is false then this is
+ // if used_passed_ is false then this is
// just the default state, otherwise will force
- // it into a state using the passed values
- void SendPositionMessage(bool use_passed, bool plunger_in,
- bool latch_in, bool brake_in) {
+ // it into a state using the plunger_in_, brake_in_, and latch_in_ values.
+ void SendPositionMessage() {
const constants::Values &values = constants::GetValues();
- ::aos::ScopedMessagePtr<::y2014::control_loops::ShooterQueue::Position>
- position = shooter_queue_.position.MakeMessage();
+ ::aos::Sender<ShooterQueue::Position>::Message position =
+ shooter_position_sender_.MakeMessage();
- if (use_passed) {
- plunger_latched_ = latch_in && plunger_in;
+ if (use_passed_) {
+ plunger_latched_ = latch_in_ && plunger_in_;
latch_piston_state_ = plunger_latched_;
- brake_piston_state_ = brake_in;
+ brake_piston_state_ = brake_in_;
}
SetPhysicalSensors(position.get());
@@ -175,22 +187,22 @@
// Simulates the claw moving for one timestep.
void Simulate() {
last_plant_position_ = GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.output.FetchLatest());
- if (shooter_queue_.output->latch_piston && !latch_piston_state_ &&
+ EXPECT_TRUE(shooter_output_fetcher_.Fetch());
+ if (shooter_output_fetcher_->latch_piston && !latch_piston_state_ &&
latch_delay_count_ <= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = 6;
- } else if (!shooter_queue_.output->latch_piston &&
+ } else if (!shooter_output_fetcher_->latch_piston &&
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = -6;
}
- if (shooter_queue_.output->brake_piston && !brake_piston_state_ &&
+ if (shooter_output_fetcher_->brake_piston && !brake_piston_state_ &&
brake_delay_count_ <= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = 5;
- } else if (!shooter_queue_.output->brake_piston &&
+ } else if (!shooter_output_fetcher_->brake_piston &&
brake_piston_state_ && brake_delay_count_ >= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = -5;
@@ -251,10 +263,16 @@
EXPECT_LE(constants::GetValues().shooter.lower_hard_limit,
GetAbsolutePosition());
- last_voltage_ = shooter_queue_.output->voltage;
- ::aos::time::IncrementMockTime(chrono::milliseconds(10));
+ last_voltage_ = shooter_output_fetcher_->voltage;
}
+ private:
+ ::aos::EventLoop *event_loop_;
+ ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
+ ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+
+ bool first_ = true;
+
// pointer to plant
const ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
@@ -271,12 +289,16 @@
// greater than zero, delaying close. less than zero delaying open
int brake_delay_count_;
- private:
- ::y2014::control_loops::ShooterQueue shooter_queue_;
+ // Overrides for testing.
+ bool use_passed_ = false;
+ bool plunger_in_ = false;
+ bool latch_in_ = false;
+ bool brake_in_ = false;
+
double initial_position_;
double last_voltage_;
- ::y2014::control_loops::ShooterQueue::Position last_position_message_;
+ ShooterQueue::Position last_position_message_;
double last_plant_position_;
};
@@ -284,35 +306,40 @@
class ShooterTestTemplated
: public ::aos::testing::ControlLoopTestTemplated<TestType> {
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.
- ::y2014::control_loops::ShooterQueue shooter_queue_;
-
- ::aos::ShmEventLoop event_loop_;
- // Create a loop and simulation plant.
- ShooterMotor shooter_motor_;
- ShooterSimulation shooter_motor_plant_;
+ ShooterTestTemplated()
+ : ::aos::testing::ControlLoopTestTemplated<TestType>(
+ // TODO(austin): I think this runs at 5 ms in real life.
+ chrono::microseconds(5000)),
+ test_event_loop_(this->MakeEventLoop()),
+ shooter_goal_fetcher_(test_event_loop_->MakeFetcher<ShooterQueue::Goal>(
+ ".y2014.control_loops.shooter_queue.goal")),
+ shooter_goal_sender_(test_event_loop_->MakeSender<ShooterQueue::Goal>(
+ ".y2014.control_loops.shooter_queue.goal")),
+ shooter_event_loop_(this->MakeEventLoop()),
+ shooter_motor_(shooter_event_loop_.get()),
+ shooter_plant_event_loop_(this->MakeEventLoop()),
+ shooter_motor_plant_(shooter_plant_event_loop_.get(), this->dt(), 0.2) {
+ }
void Reinitialize(double position) {
shooter_motor_plant_.Reinitialize(position);
}
- ShooterTestTemplated()
- : shooter_queue_(".y2014.control_loops.shooter_queue",
- ".y2014.control_loops.shooter_queue.goal",
- ".y2014.control_loops.shooter_queue.position",
- ".y2014.control_loops.shooter_queue.output",
- ".y2014.control_loops.shooter_queue.status"),
- shooter_motor_(&event_loop_),
- shooter_motor_plant_(0.2) {}
-
void VerifyNearGoal() {
- shooter_queue_.goal.FetchLatest();
- shooter_queue_.position.FetchLatest();
- double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_queue_.goal->shot_power, pos, 1e-4);
+ shooter_goal_fetcher_.Fetch();
+ const double pos = shooter_motor_plant_.GetAbsolutePosition();
+ EXPECT_NEAR(shooter_goal_fetcher_->shot_power, pos, 1e-4);
}
+
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::Fetcher<ShooterQueue::Goal> shooter_goal_fetcher_;
+ ::aos::Sender<ShooterQueue::Goal> shooter_goal_sender_;
+
+ // Create a loop and simulation plant.
+ ::std::unique_ptr<::aos::EventLoop> shooter_event_loop_;
+ ShooterMotor shooter_motor_;
+ ::std::unique_ptr<::aos::EventLoop> shooter_plant_event_loop_;
+ ShooterSimulation shooter_motor_plant_;
};
typedef ShooterTestTemplated<::testing::Test> ShooterTest;
@@ -350,99 +377,104 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
- // EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
+ RunFor(chrono::seconds(2));
+
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1200))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1200));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder()
- .shot_power(35.0)
- .shot_requested(true)
- .Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 35.0;
+ message->shot_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
bool hit_fire = false;
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(5200))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- shooter_queue_.goal.MakeWithBuilder()
- .shot_power(17.0)
- .shot_requested(false)
- .Send();
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 17.0;
+ message->shot_requested = false;
+ EXPECT_TRUE(message.Send());
}
hit_fire = true;
}
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
- EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
- pos, 0.05);
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
+ EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
}
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, FireLong) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
+
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().shot_requested(true).Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 0.0;
+ message->shot_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
bool hit_fire = false;
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(5500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- shooter_queue_.goal.MakeWithBuilder()
- .shot_requested(false)
- .Send();
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_requested = false;
+ EXPECT_TRUE(message.Send());
}
hit_fire = true;
}
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
+ EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
@@ -451,13 +483,16 @@
// Verifies that it doesn't try to go out too far if you give it a ridicilous
// power.
TEST_F(ShooterTest, LoadTooFar) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(500.0).Send();
- while (monotonic_clock::now() <
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 500.0;
+ EXPECT_TRUE(message.Send());
+ }
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(1600))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
EXPECT_LT(shooter_motor_plant_.GetAbsolutePosition(),
constants::GetValuesForTeam(kTeamNumber).shooter.upper_limit);
}
@@ -466,53 +501,55 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, MoveGoal) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
- EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().shot_power(14.0).Send();
+ RunFor(chrono::milliseconds(1500));
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 14.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(500));
+
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
TEST_F(ShooterTest, Unload) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->unload_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::seconds(8)) &&
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
@@ -522,34 +559,31 @@
// Tests that it rezeros while unloading.
TEST_F(ShooterTest, RezeroWhileUnloading) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
+
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_motor_.shooter_.offset_ += 0.01;
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(chrono::milliseconds(500));
+
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->unload_requested = true;
+ EXPECT_TRUE(message.Send());
}
- shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
-
- while (::aos::monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
::aos::monotonic_clock::time_point(chrono::seconds(10)) &&
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
@@ -559,24 +593,28 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupNegative) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->unload_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
int kicked_delay = 20;
int capped_goal_count = 0;
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(9500)) &&
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
+ RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
@@ -587,8 +625,6 @@
if (shooter_motor_.capped_goal() && kicked_delay < 0) {
++capped_goal_count;
}
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
@@ -600,24 +636,28 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupPositive) {
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::milliseconds(1500))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SetEnabled(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_.goal.MakeWithBuilder().unload_requested(true).Send();
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->unload_requested = true;
+ EXPECT_TRUE(message.Send());
+ }
int kicked_delay = 20;
int capped_goal_count = 0;
- while (monotonic_clock::now() <
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(9500)) &&
shooter_motor_.state() != ShooterMotor::STATE_READY_UNLOAD) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
+ RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_UNLOAD_MOVE) {
LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
@@ -628,8 +668,6 @@
if (shooter_motor_.capped_goal() && kicked_delay < 0) {
++capped_goal_count;
}
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
}
EXPECT_NEAR(constants::GetValues().shooter.upper_limit,
@@ -645,20 +683,20 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnDistal) {
+ SetEnabled(true);
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::seconds(2));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -666,21 +704,21 @@
// Tests that the shooter zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnProximal) {
+ SetEnabled(true);
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
- shooter_queue_.goal.MakeWithBuilder().shot_power(70.0).Send();
- while (monotonic_clock::now() <
- monotonic_clock::time_point(chrono::seconds(3))) {
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 70.0;
+ EXPECT_TRUE(message.Send());
}
+ RunFor(chrono::seconds(3));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -690,6 +728,7 @@
::testing::TestWithParam<::std::tuple<bool, bool, bool, double>>> {};
TEST_P(ShooterZeroingTest, AllDisparateStartingZero) {
+ SetEnabled(true);
bool latch = ::std::get<0>(GetParam());
bool brake = ::std::get<1>(GetParam());
bool plunger_back = ::std::get<2>(GetParam());
@@ -699,20 +738,26 @@
// latch, brake, plunger_back, start_pos);
bool initialized = false;
Reinitialize(start_pos);
- shooter_queue_.goal.MakeWithBuilder().shot_power(120.0).Send();
- while (monotonic_clock::now() <
+ {
+ ::aos::Sender<ShooterQueue::Goal>::Message message =
+ shooter_goal_sender_.MakeMessage();
+ message->shot_power = 120.0;
+ EXPECT_TRUE(message.Send());
+ }
+ while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::seconds(2))) {
- shooter_motor_plant_.SendPositionMessage(!initialized, plunger_back, latch, brake);
+ shooter_motor_plant_.set_use_passed(!initialized);
+ shooter_motor_plant_.set_plunger_in(plunger_back);
+ shooter_motor_plant_.set_latch_in(latch);
+ shooter_motor_plant_.set_brake_in(brake);
initialized = true;
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SimulateTimestep(true);
+ RunFor(dt());
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_TRUE(shooter_queue_.goal.FetchLatest());
+ EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_queue_.goal->shot_power),
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
pos, 0.05);
ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
diff --git a/y2014/control_loops/shooter/shooter_main.cc b/y2014/control_loops/shooter/shooter_main.cc
index a7b60f9..2da76c3 100644
--- a/y2014/control_loops/shooter/shooter_main.cc
+++ b/y2014/control_loops/shooter/shooter_main.cc
@@ -4,10 +4,13 @@
#include "aos/init.h"
int main() {
- ::aos::Init();
+ ::aos::InitNRT(true);
+
::aos::ShmEventLoop event_loop;
::y2014::control_loops::ShooterMotor shooter(&event_loop);
- shooter.Run();
+
+ event_loop.Run();
+
::aos::Cleanup();
return 0;
}