commit 00e4227fd886b1bd08cdcd070b4a79c06ba58cf0
author Theo Bafrali <100019017@mvla.net> Tue Feb 12 01:07:46 2019 -0800
committer Austin Schuh <austin@peloton-tech.com> Wed Feb 13 00:09:54 2019 -0800
tree 867d2402f5070f06571f3f29862af275d14e5417
parent a64a3f975cd52d0477328a75cc275912bc36be35

Implement superstructure class and tests

Change-Id: If5a2a296ada3dd625ad7eb74af106b36b36ab932

y2019/control_loops/superstructure/superstructure.h

diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 0d6765e..22dbcd6 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -2,6 +2,9 @@
 #define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
 
 #include "aos/controls/control_loop.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "y2019/constants.h"
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
 #include "y2019/control_loops/superstructure/superstructure.q.h"
 
 namespace y2019 {
@@ -16,14 +19,41 @@
       const ::std::string &name =
           ".y2019.control_loops.superstructure.superstructure_queue");
 
+  using PotAndAbsoluteEncoderSubsystem =
+      ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+          ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+          ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+  using AbsoluteEncoderSubsystem =
+      ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+          ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+          ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+  const PotAndAbsoluteEncoderSubsystem &elevator() const {
+    return elevator_;
+  }
+  const PotAndAbsoluteEncoderSubsystem &wrist() const {
+    return wrist_;
+  }
+  const AbsoluteEncoderSubsystem &intake() const {
+    return intake_;
+  }
+  const PotAndAbsoluteEncoderSubsystem &stilts() const {
+    return stilts_;
+  }
+
  protected:
-  virtual void RunIteration(
-      const SuperstructureQueue::Goal *unsafe_goal,
-      const SuperstructureQueue::Position *position,
-      SuperstructureQueue::Output *output,
-      SuperstructureQueue::Status *status) override;
+  virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
+                            const SuperstructureQueue::Position *position,
+                            SuperstructureQueue::Output *output,
+                            SuperstructureQueue::Status *status) override;
 
  private:
+  PotAndAbsoluteEncoderSubsystem elevator_;
+  PotAndAbsoluteEncoderSubsystem wrist_;
+  AbsoluteEncoderSubsystem intake_;
+  PotAndAbsoluteEncoderSubsystem stilts_;
+
+  CollisionAvoidance collision_avoidance_;
 
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };