Add shooter class and python.
Change-Id: I27b4c8f282a0b80344a7df59cf3b04569d9c8110
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
new file mode 100644
index 0000000..d63231b
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -0,0 +1,58 @@
+package(default_visibility = ["//visibility:public"])
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+cc_library(
+ name = "shooter_plants",
+ srcs = [
+ "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.cc",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plant.cc",
+ "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.cc",
+ "//y2020/control_loops/superstructure/finisher:finisher_plant.cc",
+ ],
+ hdrs = [
+ "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.h",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plant.h",
+ "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.h",
+ "//y2020/control_loops/superstructure/finisher:finisher_plant.h",
+ ],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
+
+cc_library(
+ name = "shooter",
+ srcs = [
+ "shooter.cc",
+ ],
+ hdrs = [
+ "shooter.h",
+ ],
+ deps = [
+ ":flywheel_controller",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2020/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2020/control_loops/superstructure:superstructure_output_fbs",
+ "//y2020/control_loops/superstructure:superstructure_position_fbs",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+cc_library(
+ name = "flywheel_controller",
+ srcs = [
+ "flywheel_controller.cc",
+ ],
+ hdrs = [
+ "flywheel_controller.h",
+ ],
+ deps = [
+ ":shooter_plants",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2020/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
new file mode 100644
index 0000000..0f920a4
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -0,0 +1,70 @@
+#include "y2020/control_loops/superstructure/shooter/flywheel_controller.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 {
+
+FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
+ : loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
+ history_.fill(0);
+ Y_.setZero();
+}
+
+void FlywheelController::set_goal(double angular_velocity_goal) {
+ loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
+ last_goal_ = angular_velocity_goal;
+}
+
+void FlywheelController::set_position(double current_position) {
+ // Update position in the model.
+ Y_ << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+}
+
+double FlywheelController::voltage() const { return loop_->U(0, 0); }
+
+void FlywheelController::Update(bool disabled) {
+ loop_->mutable_R() = loop_->next_R();
+ if (loop_->R(1, 0) < 1.0) {
+ // Kill power at low angular velocities.
+ disabled = true;
+ }
+
+ loop_->Correct(Y_);
+ loop_->Update(disabled);
+}
+
+flatbuffers::Offset<FlywheelControllerStatus> FlywheelController::SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ // Compute the oldest point in the history.
+ const int oldest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+ // 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));
+
+ FlywheelControllerStatusBuilder builder(*fbb);
+
+ 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();
+}
+
+} // namespace shooter
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
new file mode 100644
index 0000000..20cd681
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -0,0 +1,61 @@
+#ifndef Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+#define Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+
+#include <memory>
+
+#include "aos/controls/control_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_integral_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_integral_plant.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace shooter {
+
+// Handles the velocity control of each flywheel.
+class FlywheelController {
+ public:
+ FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop);
+
+ // 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);
+
+ // Populates the status structure.
+ flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Returns the instantaneous velocity.
+ double velocity() const { return loop_->X_hat(1, 0); }
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled);
+
+ private:
+ // The current sensor measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // The control loop.
+ ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+
+ // History array for calculating a filtered angular velocity.
+ static constexpr int kHistoryLength = 10;
+ ::std::array<double, kHistoryLength> history_;
+ ptrdiff_t history_position_ = 0;
+ double last_goal_ = 0;
+
+ DISALLOW_COPY_AND_ASSIGN(FlywheelController);
+};
+
+} // namespace shooter
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
+
+#endif // Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
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
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
new file mode 100644
index 0000000..88bcb3b
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -0,0 +1,37 @@
+#ifndef Y2020_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+#define Y2020_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+
+#include "aos/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2020/control_loops/superstructure/shooter/flywheel_controller.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"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace shooter {
+
+// Handles all flywheels together.
+class Shooter {
+ public:
+ Shooter();
+
+ flatbuffers::Offset<ShooterStatus> RunIteration(
+ const ShooterGoal *goal, const ShooterPosition *position,
+ flatbuffers::FlatBufferBuilder *fbb, OutputT *output);
+
+ private:
+ FlywheelController finisher_, accelerator_left_, accelerator_right_;
+
+ DISALLOW_COPY_AND_ASSIGN(Shooter);
+};
+
+} // namespace shooter
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
+
+#endif // Y2020_CONTROL_LOOPS_SHOOTER_SHOOTER_H_