Added turret and intake code and tests.
Tests are from Adam.
Change-Id: I5a89700cfe2e9983771b4523facc302243b5dc50
diff --git a/y2017/control_loops/superstructure/turret/BUILD b/y2017/control_loops/superstructure/turret/BUILD
index 70b9508..8969693 100644
--- a/y2017/control_loops/superstructure/turret/BUILD
+++ b/y2017/control_loops/superstructure/turret/BUILD
@@ -27,3 +27,20 @@
'//frc971/control_loops:state_feedback_loop',
],
)
+
+cc_library(
+ name = 'turret',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'turret.cc',
+ ],
+ hdrs = [
+ 'turret.h',
+ ],
+ deps = [
+ ':turret_plants',
+ '//frc971/control_loops:profiled_subsystem',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017:constants',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/turret/turret.cc b/y2017/control_loops/superstructure/turret/turret.cc
new file mode 100644
index 0000000..9616dff
--- /dev/null
+++ b/y2017/control_loops/superstructure/turret/turret.cc
@@ -0,0 +1,125 @@
+#include "y2017/control_loops/superstructure/turret/turret.h"
+
+#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/turret/turret_integral_plant.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+constexpr double Turret::kZeroingVoltage;
+constexpr double Turret::kOperatingVoltage;
+
+Turret::Turret()
+ : profiled_subsystem_(
+ ::std::unique_ptr<
+ ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+ new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+ 3, 1, 1>(MakeIntegralTurretLoop())),
+ constants::GetValues().turret.zeroing,
+ constants::Values::kTurretRange, 7.0, 50.0) {}
+
+void Turret::Reset() {
+ state_ = State::UNINITIALIZED;
+ profiled_subsystem_.Reset();
+}
+
+void Turret::Iterate(
+ const control_loops::TurretGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position, double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ bool disable = output == nullptr;
+ profiled_subsystem_.Correct(*position);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Wait in the uninitialized state until the turret is initialized.
+ LOG(DEBUG, "Uninitialized, waiting for turret\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:
+ // Now, zero by actively holding still.
+ if (profiled_subsystem_.zeroed()) {
+ state_ = State::RUNNING;
+ } else if (disable) {
+ state_ = State::DISABLED_INITIALIZED;
+ }
+ 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 (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 turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/superstructure/turret/turret.h b/y2017/control_loops/superstructure/turret/turret.h
new file mode 100644
index 0000000..5499d92
--- /dev/null
+++ b/y2017/control_loops/superstructure/turret/turret.h
@@ -0,0 +1,53 @@
+#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_TURRET_H_
+#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_TURRET_H_
+
+#include "frc971/control_loops/profiled_subsystem.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+class Turret {
+ public:
+ Turret();
+ 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::TurretGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position,
+ double *output,
+ ::frc971::control_loops::AbsoluteProfiledJointStatus *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<
+ ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator>
+ profiled_subsystem_;
+};
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_TURRET_H_