Added shooter and tests.
Added the shooter into the main superstructure and added tests.
Change-Id: I6c9afe3c74a08251854805050c40fafdca90fba8
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 =