Added shooter and tests.
Added the shooter into the main superstructure and added tests.
Change-Id: I6c9afe3c74a08251854805050c40fafdca90fba8
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 87ee689..de15415 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -63,6 +63,7 @@
"//aos/controls:control_loop",
"//aos/events:event_loop",
"//y2020:constants",
+ "//y2020/control_loops/superstructure/shooter",
],
)
@@ -102,6 +103,7 @@
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2020/control_loops/superstructure/hood:hood_plants",
"//y2020/control_loops/superstructure/intake:intake_plants",
+ "//y2020/control_loops/superstructure/shooter:shooter_plants",
],
)
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 0f920a4..77fb769 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -13,7 +13,8 @@
FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
: loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
- history_.fill(0);
+ history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
+ 0, ::aos::monotonic_clock::epoch()));
Y_.setZero();
}
@@ -22,12 +23,16 @@
last_goal_ = angular_velocity_goal;
}
-void FlywheelController::set_position(double current_position) {
+void FlywheelController::set_position(
+ double current_position,
+ const aos::monotonic_clock::time_point position_timestamp) {
// Update position in the model.
Y_ << current_position;
// Add the position to the history.
- history_[history_position_] = current_position;
+ history_[history_position_] =
+ std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
+ position_timestamp);
history_position_ = (history_position_ + 1) % kHistoryLength;
}
@@ -50,15 +55,20 @@
const int oldest_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+ const double total_loop_time = ::aos::time::DurationInSeconds(
+ std::get<1>(history_[history_position_]) -
+ std::get<1>(history_[oldest_history_position]));
+
+ const double distance_traveled =
+ std::get<0>(history_[history_position_]) -
+ std::get<0>(history_[oldest_history_position]);
+
// Compute the distance moved over that time period.
- const double avg_angular_velocity =
- (history_[oldest_history_position] - history_[history_position_]) /
- (::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
- static_cast<double>(kHistoryLength - 1));
+ avg_angular_velocity_ = (distance_traveled) / (total_loop_time);
FlywheelControllerStatusBuilder builder(*fbb);
- builder.add_avg_angular_velocity(avg_angular_velocity);
+ builder.add_avg_angular_velocity(avg_angular_velocity_);
builder.add_angular_velocity(loop_->X_hat(1, 0));
builder.add_angular_velocity_goal(last_goal_);
return builder.Finish();
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index 20cd681..5853097 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -23,7 +23,8 @@
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
// Sets the current encoder position in radians
- void set_position(double current_position);
+ void set_position(double current_position,
+ const aos::monotonic_clock::time_point position_timestamp);
// Populates the status structure.
flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
@@ -38,6 +39,8 @@
// Executes the control loop for a cycle.
void Update(bool disabled);
+ double avg_angular_velocity() { return avg_angular_velocity_; }
+
private:
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
@@ -46,8 +49,14 @@
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 10;
- ::std::array<double, kHistoryLength> history_;
+ ::std::array<std::pair<double, ::aos::monotonic_clock::time_point>,
+ kHistoryLength>
+ history_;
ptrdiff_t history_position_ = 0;
+
+ // Average velocity logging.
+ double avg_angular_velocity_;
+
double last_goal_ = 0;
DISALLOW_COPY_AND_ASSIGN(FlywheelController);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 6a1d201..25f6a6a 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -11,29 +11,59 @@
namespace superstructure {
namespace shooter {
+namespace {
+const double kVelocityTolerance = 0.01;
+} // namespace
+
Shooter::Shooter()
: finisher_(finisher::MakeIntegralFinisherLoop()),
accelerator_left_(accelerator::MakeIntegralAcceleratorLoop()),
accelerator_right_(accelerator::MakeIntegralAcceleratorLoop()) {}
+bool Shooter::UpToSpeed(const ShooterGoal *goal) {
+ return (
+ std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
+ kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() -
+ accelerator_left_.avg_angular_velocity()) < kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() -
+ accelerator_right_.avg_angular_velocity()) < kVelocityTolerance &&
+ std::abs(goal->velocity_finisher() - finisher_.velocity()) < kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
+ kVelocityTolerance &&
+ std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
+ kVelocityTolerance);
+}
+
flatbuffers::Offset<ShooterStatus> Shooter::RunIteration(
const ShooterGoal *goal, const ShooterPosition *position,
- flatbuffers::FlatBufferBuilder *fbb, OutputT *output) {
- if (goal) {
- // Update position/goal for our two shooter sides.
- finisher_.set_goal(goal->velocity_finisher());
- accelerator_left_.set_goal(goal->velocity_accelerator());
- accelerator_right_.set_goal(goal->velocity_accelerator());
- }
-
- finisher_.set_position(position->theta_finisher());
- accelerator_left_.set_position(position->theta_accelerator_left());
- accelerator_right_.set_position(position->theta_accelerator_right());
+ flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
+ const aos::monotonic_clock::time_point position_timestamp) {
+ // Update position, output, and status for our two shooter sides.
+ finisher_.set_position(position->theta_finisher(), position_timestamp);
+ accelerator_left_.set_position(position->theta_accelerator_left(),
+ position_timestamp);
+ accelerator_right_.set_position(position->theta_accelerator_right(),
+ position_timestamp);
finisher_.Update(output == nullptr);
accelerator_left_.Update(output == nullptr);
accelerator_right_.Update(output == nullptr);
+ // Update goal.
+ if (goal) {
+ finisher_.set_goal(goal->velocity_finisher());
+ accelerator_left_.set_goal(goal->velocity_accelerator());
+ accelerator_right_.set_goal(goal->velocity_accelerator());
+
+ if (UpToSpeed(goal) && goal->velocity_finisher() > kVelocityTolerance &&
+ goal->velocity_accelerator() > kVelocityTolerance) {
+ ready_ = true;
+ } else {
+ ready_ = false;
+ }
+ }
+
flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
finisher_.SetStatus(fbb);
flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset =
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index 88bcb3b..f72eeeb 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -21,11 +21,17 @@
flatbuffers::Offset<ShooterStatus> RunIteration(
const ShooterGoal *goal, const ShooterPosition *position,
- flatbuffers::FlatBufferBuilder *fbb, OutputT *output);
+ flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
+ const aos::monotonic_clock::time_point position_timestamp);
+
+ bool ready() { return ready_; }
private:
FlywheelController finisher_, accelerator_left_, accelerator_right_;
+ bool UpToSpeed(const ShooterGoal *goal);
+ bool ready_ = false;
+
DISALLOW_COPY_AND_ASSIGN(Shooter);
};
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index c2512db..39c913e 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -15,7 +15,8 @@
name),
hood_(constants::GetValues().hood),
intake_joint_(constants::GetValues().intake),
- turret_(constants::GetValues().turret.subsystem_params) {
+ turret_(constants::GetValues().turret.subsystem_params),
+ shooter_() {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -30,6 +31,9 @@
turret_.Reset();
}
+ const aos::monotonic_clock::time_point position_timestamp =
+ event_loop()->context().monotonic_event_time;
+
OutputT output_struct;
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -52,6 +56,12 @@
output != nullptr ? &(output_struct.turret_voltage) : nullptr,
status->fbb());
+ flatbuffers::Offset<ShooterStatus> shooter_status_offset =
+ shooter_.RunIteration(
+ unsafe_goal != nullptr ? unsafe_goal->shooter() : nullptr,
+ position->shooter(), status->fbb(),
+ output != nullptr ? &(output_struct) : nullptr, position_timestamp);
+
climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
bool zeroed;
@@ -81,6 +91,7 @@
status_builder.add_hood(hood_status_offset);
status_builder.add_intake(intake_status_offset);
status_builder.add_turret(turret_status_offset);
+ status_builder.add_shooter(shooter_status_offset);
status->Send(status_builder.Finish());
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 2bc0cda..d8ce917 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -4,6 +4,7 @@
#include "aos/controls/control_loop.h"
#include "aos/events/event_loop.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
@@ -32,6 +33,7 @@
const AbsoluteEncoderSubsystem &hood() const { return hood_; }
const AbsoluteEncoderSubsystem &intake_joint() const { return intake_joint_; }
const PotAndAbsoluteEncoderSubsystem &turret() const { return turret_; }
+ const shooter::Shooter &shooter() const { return shooter_; }
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
@@ -42,6 +44,7 @@
AbsoluteEncoderSubsystem hood_;
AbsoluteEncoderSubsystem intake_joint_;
PotAndAbsoluteEncoderSubsystem turret_;
+ shooter::Shooter shooter_;
Climber climber_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 458a771..9df845a 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -9,6 +9,8 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
#include "y2020/control_loops/superstructure/hood/hood_plant.h"
#include "y2020/control_loops/superstructure/intake/intake_plant.h"
#include "y2020/control_loops/superstructure/superstructure.h"
@@ -34,6 +36,25 @@
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
+class FlywheelPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit FlywheelPlant(StateFeedbackPlant<2, 1, 1> &&other)
+ : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+ void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+ }
+
+ double voltage_offset() const { return voltage_offset_; }
+ void set_voltage_offset(double voltage_offset) {
+ voltage_offset_ = voltage_offset;
+ }
+
+ private:
+ double voltage_offset_ = 0.0;
+};
+
// Class which simulates the superstructure and sends out queue messages with
// the position.
class SuperstructureSimulation {
@@ -57,7 +78,12 @@
turret_plant_(new CappedTestPlant(turret::MakeTurretPlant())),
turret_encoder_(constants::GetValues()
.turret.subsystem_params.zeroing_constants
- .one_revolution_distance) {
+ .one_revolution_distance),
+ accelerator_left_plant_(
+ new FlywheelPlant(accelerator::MakeAcceleratorPlant())),
+ accelerator_right_plant_(
+ new FlywheelPlant(accelerator::MakeAcceleratorPlant())),
+ finisher_plant_(new FlywheelPlant(finisher::MakeFinisherPlant())) {
InitializeHoodPosition(constants::Values::kHoodRange().upper);
InitializeIntakePosition(constants::Values::kIntakeRange().upper);
InitializeTurretPosition(constants::Values::kTurretRange().middle());
@@ -104,6 +130,14 @@
.measured_absolute_position);
}
+ flatbuffers::Offset<ShooterPosition> shooter_pos_offset(
+ ShooterPositionBuilder *builder) {
+ builder->add_theta_finisher(finisher_plant_->Y(0, 0));
+ builder->add_theta_accelerator_left(accelerator_left_plant_->Y(0, 0));
+ builder->add_theta_accelerator_right(accelerator_right_plant_->Y(0, 0));
+ return builder->Finish();
+ }
+
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
::aos::Sender<Position>::Builder builder =
@@ -124,11 +158,17 @@
flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
turret_encoder_.GetSensorValues(&turret_builder);
+ ShooterPosition::Builder shooter_builder =
+ builder.MakeBuilder<ShooterPosition>();
+ flatbuffers::Offset<ShooterPosition> shooter_offset =
+ shooter_pos_offset(&shooter_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_hood(hood_offset);
position_builder.add_intake_joint(intake_offset);
position_builder.add_turret(turret_offset);
+ position_builder.add_shooter(shooter_offset);
builder.Send(position_builder.Finish());
}
@@ -142,6 +182,16 @@
double turret_position() const { return turret_plant_->X(0, 0); }
double turret_velocity() const { return turret_plant_->X(1, 0); }
+ double accelerator_left_velocity() const {
+ return accelerator_left_plant_->X(1, 0);
+ }
+
+ double accelerator_right_velocity() const {
+ return accelerator_right_plant_->X(1, 0);
+ }
+
+ double finisher_velocity() const { return finisher_plant_->X(1, 0); }
+
// Simulates the superstructure for a single timestep.
void Simulate() {
const double last_hood_velocity = hood_velocity();
@@ -193,9 +243,26 @@
turret_U << superstructure_output_fetcher_->turret_voltage() +
turret_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> accelerator_left_U;
+ accelerator_left_U
+ << superstructure_output_fetcher_->accelerator_left_voltage() +
+ accelerator_left_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> accelerator_right_U;
+ accelerator_right_U
+ << superstructure_output_fetcher_->accelerator_right_voltage() +
+ accelerator_right_plant_->voltage_offset();
+
+ ::Eigen::Matrix<double, 1, 1> finisher_U;
+ finisher_U << superstructure_output_fetcher_->finisher_voltage() +
+ finisher_plant_->voltage_offset();
+
hood_plant_->Update(hood_U);
intake_plant_->Update(intake_U);
turret_plant_->Update(turret_U);
+ accelerator_left_plant_->Update(accelerator_left_U);
+ accelerator_right_plant_->Update(accelerator_right_U);
+ finisher_plant_->Update(finisher_U);
const double position_hood = hood_plant_->Y(0, 0);
const double position_intake = intake_plant_->Y(0, 0);
@@ -280,6 +347,10 @@
::std::unique_ptr<CappedTestPlant> turret_plant_;
PositionSensorSimulator turret_encoder_;
+ ::std::unique_ptr<FlywheelPlant> accelerator_left_plant_;
+ ::std::unique_ptr<FlywheelPlant> accelerator_right_plant_;
+ ::std::unique_ptr<FlywheelPlant> finisher_plant_;
+
// The acceleration limits to check for while moving.
double peak_hood_acceleration_ = 1e10;
double peak_intake_acceleration_ = 1e10;
@@ -336,6 +407,48 @@
EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
superstructure_status_fetcher_->turret()->position(), 0.001);
}
+
+ if (superstructure_goal_fetcher_->has_shooter()) {
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+ superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+ superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->velocity_finisher(),
+ superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+ superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->avg_angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+ superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->avg_angular_velocity(),
+ 0.001);
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->velocity_finisher(),
+ superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->avg_angular_velocity(),
+ 0.001);
+ }
}
void CheckIfZeroed() {
@@ -352,8 +465,8 @@
// 2 Seconds
ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
- // Since there is a delay when sending running, make sure we have a status
- // before checking it.
+ // Since there is a delay when sending running, make sure we have a
+ // status before checking it.
} while (superstructure_status_fetcher_.get() == nullptr ||
!superstructure_status_fetcher_.get()->zeroed());
}
@@ -374,7 +487,8 @@
SuperstructureSimulation superstructure_plant_;
};
-// Tests that the superstructure does nothing when the goal is to remain still.
+// Tests that the superstructure does nothing when the goal is to remain
+// still.
TEST_F(SuperstructureTest, DoesNothing) {
SetEnabled(true);
superstructure_plant_.InitializeHoodPosition(
@@ -399,11 +513,15 @@
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_shooter(shooter_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -439,11 +557,15 @@
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 300.0, 300.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_shooter(shooter_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -453,8 +575,10 @@
VerifyNearGoal();
}
+
// Makes sure that the voltage on a motor is properly pulled back after
-// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
+// saturation such that we don't get weird or bad (e.g. oscillating)
+// behaviour.
TEST_F(SuperstructureTest, SaturationTest) {
SetEnabled(true);
// Zero it before we move.
@@ -474,11 +598,15 @@
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_shooter(shooter_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -504,11 +632,15 @@
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_hood(hood_offset);
goal_builder.add_intake(intake_offset);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_shooter(shooter_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
@@ -526,6 +658,81 @@
VerifyNearGoal();
}
+// Tests the shooter can spin up correctly.
+TEST_F(SuperstructureTest, SpinUp) {
+ SetEnabled(true);
+ superstructure_plant_.InitializeHoodPosition(
+ constants::Values::kHoodRange().upper);
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange().upper);
+
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kHoodRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+
+ // Start up the accelerator and make sure both run.
+ shooter_builder.add_velocity_accelerator(20.0);
+ shooter_builder.add_velocity_finisher(20.0);
+
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ // Give it a lot of time to get there.
+ RunFor(chrono::seconds(8));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.7,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.7,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+ flatbuffers::Offset<ShooterGoal> shooter_offset =
+ CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ // Give it a lot of time to get there.
+ RunFor(chrono::seconds(9));
+
+ VerifyNearGoal();
+}
+
// Tests that the loop zeroes when run for a while without a goal.
TEST_F(SuperstructureTest, ZeroNoGoal) {
SetEnabled(true);