Add shooter class and python.
Change-Id: I27b4c8f282a0b80344a7df59cf3b04569d9c8110
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
new file mode 100644
index 0000000..6a1d201
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -0,0 +1,62 @@
+#include "y2020/control_loops/superstructure/shooter/shooter.h"
+
+#include <chrono>
+
+#include "aos/logging/logging.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace shooter {
+
+Shooter::Shooter()
+ : finisher_(finisher::MakeIntegralFinisherLoop()),
+ accelerator_left_(accelerator::MakeIntegralAcceleratorLoop()),
+ accelerator_right_(accelerator::MakeIntegralAcceleratorLoop()) {}
+
+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());
+
+ finisher_.Update(output == nullptr);
+ accelerator_left_.Update(output == nullptr);
+ accelerator_right_.Update(output == nullptr);
+
+ flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
+ finisher_.SetStatus(fbb);
+ flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset =
+ accelerator_left_.SetStatus(fbb);
+ flatbuffers::Offset<FlywheelControllerStatus>
+ accelerator_right_status_offset = accelerator_right_.SetStatus(fbb);
+
+ ShooterStatusBuilder status_builder(*fbb);
+
+ status_builder.add_finisher(finisher_status_offset);
+ status_builder.add_accelerator_left(accelerator_left_status_offset);
+ status_builder.add_accelerator_right(accelerator_right_status_offset);
+
+ if (output) {
+ output->finisher_voltage = finisher_.voltage();
+ output->accelerator_left_voltage = accelerator_left_.voltage();
+ output->accelerator_right_voltage = accelerator_right_.voltage();
+ }
+
+ return status_builder.Finish();
+}
+
+} // namespace shooter
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020