commit cb60e29f4bfe2d1877f841817b6e726ee2831c2e
author Austin Schuh <austin.linux@gmail.com> Sun Jan 01 16:07:31 2017 -0800
committer Austin Schuh <austin.linux@gmail.com> Mon Jan 02 20:42:02 2017 -0800
tree bf0a1d4bb672c629fe871a74a4ee1ddd6f90de21
parent f2a50ba841f843e01da373aaad271974ea4fa99e

Factor out a generic "standard subsystem"

This is a work in progress.  More refactoring is needed.

Change-Id: I02440af5111e2810e172c8a23c2134ab0571c4d5

y2016/control_loops/superstructure/superstructure_controls.cc

diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index eb6432d..7d4dea5 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -23,16 +23,22 @@
     return default_value;
   }
 }
+
+enum ArmIndices { kShoulderIndex = 0, kWristIndex = 1 };
+
 }  // namespace
 
 // Intake
 Intake::Intake()
-    : loop_(new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>(
-          ::y2016::control_loops::superstructure::MakeIntegralIntakeLoop())),
+    : ProfiledSubsystem(
+          ::std::unique_ptr<
+              ::frc971::control_loops::SimpleCappedStateFeedbackLoop<3, 1, 1>>(
+              new ::frc971::control_loops::SimpleCappedStateFeedbackLoop<
+                  3, 1, 1>(::y2016::control_loops::superstructure::
+                               MakeIntegralIntakeLoop()))),
       estimator_(constants::GetValues().intake.zeroing),
       profile_(::aos::controls::kLoopFrequency) {
   Y_.setZero();
-  unprofiled_goal_.setZero();
   offset_.setZero();
   AdjustProfile(0.0, 0.0);
 }
@@ -66,9 +72,9 @@
     }
   }
 
-  if (!zeroed_ && estimator_.zeroed()) {
+  if (!zeroed(0) && estimator_.zeroed()) {
     UpdateIntakeOffset(estimator_.offset());
-    zeroed_ = true;
+    set_zeroed(0, true);
   }
 
   Y_ << position.encoder;
@@ -148,10 +154,8 @@
       UseUnlessZero(max_angular_acceleration, 10.0));
 }
 
-void Intake::Reset() {
+void Intake::DoReset() {
   estimator_.Reset();
-  initialized_ = false;
-  zeroed_ = false;
 }
 
 EstimatorState Intake::IntakeEstimatorState() {
@@ -162,15 +166,14 @@
 }
 
 Arm::Arm()
-    : loop_(new ArmControlLoop(
-          ::y2016::control_loops::superstructure::MakeIntegralArmLoop())),
+    : ProfiledSubsystem(::std::unique_ptr<ArmControlLoop>(new ArmControlLoop(
+          ::y2016::control_loops::superstructure::MakeIntegralArmLoop()))),
       shoulder_profile_(::aos::controls::kLoopFrequency),
       wrist_profile_(::aos::controls::kLoopFrequency),
       shoulder_estimator_(constants::GetValues().shoulder.zeroing),
       wrist_estimator_(constants::GetValues().wrist.zeroing) {
   Y_.setZero();
   offset_.setZero();
-  unprofiled_goal_.setZero();
   AdjustProfile(0.0, 0.0, 0.0, 0.0);
 }
 
@@ -238,13 +241,13 @@
     }
   }
 
-  if (!shoulder_zeroed_ && shoulder_estimator_.zeroed()) {
+  if (!zeroed(kShoulderIndex) && shoulder_estimator_.zeroed()) {
     UpdateShoulderOffset(shoulder_estimator_.offset());
-    shoulder_zeroed_ = true;
+    set_zeroed(kShoulderIndex, true);
   }
-  if (!wrist_zeroed_ && wrist_estimator_.zeroed()) {
+  if (!zeroed(kWristIndex) && wrist_estimator_.zeroed()) {
     UpdateWristOffset(wrist_estimator_.offset());
-    wrist_zeroed_ = true;
+    set_zeroed(kWristIndex, true);
   }
 
   {
@@ -373,12 +376,9 @@
   loop_->set_max_voltage(1, wrist_max_voltage);
 }
 
-void Arm::Reset() {
+void Arm::DoReset() {
   shoulder_estimator_.Reset();
   wrist_estimator_.Reset();
-  initialized_ = false;
-  shoulder_zeroed_ = false;
-  wrist_zeroed_ = false;
 }
 
 EstimatorState Arm::ShoulderEstimatorState() {