Initial hood implementation.
- Zeroing works, control loop converges.
Change-Id: Icf28f17f7623dbe565379ae14c56e699c2631a8e
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index d0603ba..de419c8 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -13,3 +13,19 @@
'//frc971/control_loops:queues',
],
)
+
+cc_library(
+ name = 'superstructure_lib',
+ srcs = [
+ 'superstructure.cc',
+ ],
+ hdrs = [
+ 'superstructure.h',
+ ],
+ deps = [
+ ':superstructure_queue',
+ '//aos/common/controls:control_loop',
+ '//y2017/control_loops/superstructure/hood',
+ '//y2017:constants',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/hood/BUILD b/y2017/control_loops/superstructure/hood/BUILD
index 7b03141..17136c5 100644
--- a/y2017/control_loops/superstructure/hood/BUILD
+++ b/y2017/control_loops/superstructure/hood/BUILD
@@ -29,3 +29,19 @@
'//frc971/control_loops:state_feedback_loop',
],
)
+
+cc_library(
+ name = 'hood',
+ srcs = [
+ 'hood.cc',
+ ],
+ hdrs = [
+ 'hood.h',
+ ],
+ deps = [
+ ':hood_plants',
+ '//frc971/control_loops:profiled_subsystem',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017:constants',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/hood/hood.cc b/y2017/control_loops/superstructure/hood/hood.cc
new file mode 100644
index 0000000..8af4a9e
--- /dev/null
+++ b/y2017/control_loops/superstructure/hood/hood.cc
@@ -0,0 +1,157 @@
+#include "y2017/control_loops/superstructure/hood/hood.h"
+
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/hood/hood_integral_plant.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace hood {
+
+constexpr double Hood::kZeroingVoltage;
+constexpr double Hood::kOperatingVoltage;
+
+Hood::Hood()
+ : profiled_subsystem_(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 3, 1, 1>(MakeIntegralHoodLoop())),
+ constants::GetValues().hood.zeroing, constants::Values::kHoodRange,
+ 0.5, 10.0) {}
+
+void Hood::Reset() {
+ state_ = State::UNINITIALIZED;
+ profiled_subsystem_.Reset();
+}
+
+void Hood::Iterate(const control_loops::HoodGoal *unsafe_goal,
+ const ::frc971::PotAndIndexPosition *position,
+ double *output,
+ ::frc971::control_loops::ProfiledJointStatus *status) {
+ bool disable = output == nullptr;
+ profiled_subsystem_.Correct(*position);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Wait in the uninitialized state until the hood is initialized.
+ LOG(DEBUG, "Uninitialized, waiting for hood\n");
+ if (profiled_subsystem_.initialized()) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ disable = true;
+ break;
+
+ case State::DISABLED_INITIALIZED:
+ // Wait here until we are either fully zeroed while disabled, or we become
+ // enabled.
+ if (disable) {
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ }
+ } else {
+ state_ = State::ZEROING;
+ }
+
+ // Set the goals to where we are now so when we start back up, we don't
+ // jump.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ // Set up the profile to be the zeroing profile.
+ profiled_subsystem_.AdjustProfile(0.10, 1);
+
+ // We are not ready to start doing anything yet.
+ disable = true;
+ break;
+
+ case State::ZEROING:
+ if (profiled_subsystem_.zeroed()) {
+ // Move the goal to the current goal so we stop moving.
+ profiled_subsystem_.set_unprofiled_goal(profiled_subsystem_.goal(0, 0));
+ state_ = State::RUNNING;
+ } else if (disable) {
+ state_ = State::DISABLED_INITIALIZED;
+ } else {
+ // Seek between the two soft limits.
+ if (profiled_subsystem_.position() >
+ (profiled_subsystem_.range().lower +
+ profiled_subsystem_.range().upper) /
+ 2.0) {
+ // We are above the middle.
+ if (profiled_subsystem_.goal(1, 0) > 0) {
+ // And moving up. Keep going to the upper soft limit until we
+ // arrive.
+ profiled_subsystem_.set_unprofiled_goal(
+ profiled_subsystem_.range().upper);
+ } else {
+ // And no longer moving. Go down to the lower soft limit.
+ profiled_subsystem_.set_unprofiled_goal(
+ profiled_subsystem_.range().lower);
+ }
+ } else {
+ // We are below the middle.
+ if (profiled_subsystem_.goal(1, 0) < 0) {
+ // And moving down. Keep going to the lower soft limit until we
+ // arrive.
+ profiled_subsystem_.set_unprofiled_goal(
+ profiled_subsystem_.range().lower);
+ } else {
+ // And no longer moving. Go up to the upper soft limit.
+ profiled_subsystem_.set_unprofiled_goal(
+ profiled_subsystem_.range().upper);
+ }
+ }
+ }
+ break;
+
+ case State::RUNNING: {
+ if (disable) {
+ // Reset the profile to the current position so it starts from here when
+ // we get re-enabled.
+ profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
+ }
+
+ // If we have a goal, go to it. Otherwise stay where we are.
+ if (unsafe_goal) {
+ profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
+ profiled_subsystem_.set_unprofiled_goal(unsafe_goal->angle);
+ }
+
+ // ESTOP if we hit the hard limits.
+ if (profiled_subsystem_.CheckHardLimits()) {
+ state_ = State::ESTOP;
+ }
+ } break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ disable = true;
+ break;
+ }
+
+ // Set the voltage limits.
+ const double max_voltage =
+ (state_ == State::RUNNING) ? kOperatingVoltage : kZeroingVoltage;
+
+ profiled_subsystem_.set_max_voltage({{max_voltage}});
+
+ // Calculate the loops for a cycle.
+ profiled_subsystem_.Update(disable);
+
+ // Write out all the voltages.
+ if (output) {
+ *output = profiled_subsystem_.voltage();
+ }
+
+ // Save debug/internal state.
+ // TODO(austin): Save more.
+ status->zeroed = profiled_subsystem_.zeroed();
+
+ profiled_subsystem_.PopulateStatus(status);
+ status->estopped = (state_ == State::ESTOP);
+ status->state = static_cast<int32_t>(state_);
+}
+
+} // namespace hood
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
new file mode 100644
index 0000000..b96d384
--- /dev/null
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -0,0 +1,50 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_HOOD_HOOD_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_HOOD_HOOD_H_
+
+#include "frc971/control_loops/profiled_subsystem.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace hood {
+
+class Hood {
+ public:
+ Hood();
+ double goal(int row, int col) const {
+ return profiled_subsystem_.goal(row, col);
+ }
+
+ // The zeroing and operating voltages.
+ static constexpr double kZeroingVoltage = 2.5;
+ static constexpr double kOperatingVoltage = 12.0;
+
+ void Iterate(const control_loops::HoodGoal *unsafe_goal,
+ const ::frc971::PotAndIndexPosition *position, double *output,
+ ::frc971::control_loops::ProfiledJointStatus *status);
+
+ void Reset();
+
+ enum class State : int32_t{
+ UNINITIALIZED,
+ DISABLED_INITIALIZED,
+ ZEROING,
+ RUNNING,
+ ESTOP,
+ };
+
+ State state() const { return state_; }
+
+ private:
+ State state_;
+
+ ::frc971::control_loops::SingleDOFProfiledSubsystem<> profiled_subsystem_;
+};
+
+} // namespace hood
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_HOOD_HOOD_H_
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..424af9a
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,35 @@
+#include "y2017/control_loops/superstructure/superstructure.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/hood/hood.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+
+Superstructure::Superstructure(
+ control_loops::SuperstructureQueue *superstructure_queue)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
+ superstructure_queue) {}
+
+void Superstructure::RunIteration(
+ const control_loops::SuperstructureQueue::Goal *unsafe_goal,
+ const control_loops::SuperstructureQueue::Position *position,
+ control_loops::SuperstructureQueue::Output *output,
+ control_loops::SuperstructureQueue::Status *status) {
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ hood_.Reset();
+ }
+
+ hood_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->hood) : nullptr,
+ &(position->hood),
+ output != nullptr ? &(output->voltage_hood) : nullptr,
+ &(status->hood));
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..036f0f8
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -0,0 +1,70 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2017/control_loops/superstructure/hood/hood.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ public:
+ explicit Superstructure(
+ control_loops::SuperstructureQueue *my_superstructure =
+ &control_loops::superstructure_queue);
+
+ enum State {
+ // Wait for all the filters to be ready before starting the initialization
+ // process.
+ UNINITIALIZED = 0,
+
+ // We now are ready to decide how to zero. Decide what to do once we are
+ // enabled.
+ DISABLED_INITIALIZED = 1,
+
+ ZEROING = 2,
+ // Run with full power.
+ RUNNING = 3,
+ // Internal error caused the superstructure to abort.
+ ESTOP = 4,
+ };
+
+ bool IsRunning() const { return state_ == RUNNING; }
+
+ State state() const { return state_; }
+
+ // Returns the value to move the joint to such that it will stay below
+ // reference_angle starting at current_angle, but move at least move_distance
+ static double MoveButKeepBelow(double reference_angle, double current_angle,
+ double move_distance);
+ // Returns the value to move the joint to such that it will stay above
+ // reference_angle starting at current_angle, but move at least move_distance
+ static double MoveButKeepAbove(double reference_angle, double current_angle,
+ double move_distance);
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::SuperstructureQueue::Goal *unsafe_goal,
+ const control_loops::SuperstructureQueue::Position *position,
+ control_loops::SuperstructureQueue::Output *output,
+ control_loops::SuperstructureQueue::Status *status) override;
+
+ private:
+ hood::Hood hood_;
+
+ State state_ = UNINITIALIZED;
+
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_