commit 55aa3baa844aa85d39bc95d597a0b79bbed55ade
author Henry Speiser <henry@speiser.net> Mon Feb 21 23:21:12 2022 -0800
committer Henry Speiser <henry@speiser.net> Wed Feb 23 18:38:53 2022 -0800
tree 544e8deab19a3536fcfd19ccdf63696d59281c45
parent 0e37f59ce4d78fdcc64f7a46e5e453c18b919f2a

Add intake and turret code plus superstructure tests

Signed-off-by: Milo Lin <100027790@mvla.net>
Change-Id: I9885bd1e839ba0356147606415ae915cd295faf6

Change-Id: I33bc83673645869e255136198c0789f722c881a0
Signed-off-by: Siddhartha Chatterjee <ninja.siddhartha@gmail.com>
Signed-off-by: Griffin Bui <griffinbui+gerrit@gmail.com>
Signed-off-by: Henry Speiser <henry@speiser.net>

y2022/control_loops/superstructure/superstructure.h

diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 4c03a08..d97befc 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -3,6 +3,7 @@
 
 #include "aos/events/event_loop.h"
 #include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "y2022/constants.h"
 #include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_output_generated.h"
@@ -21,9 +22,28 @@
           ::frc971::zeroing::RelativeEncoderZeroingEstimator,
           ::frc971::control_loops::RelativeEncoderProfiledJointStatus>;
 
+  using PotAndAbsoluteEncoderSubsystem =
+      ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+          ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+          ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
   explicit Superstructure(::aos::EventLoop *event_loop,
+                         std::shared_ptr<const constants::Values> values,
                           const ::std::string &name = "/superstructure");
 
+  inline const PotAndAbsoluteEncoderSubsystem &intake_front() const {
+    return intake_front_;
+  }
+  inline const PotAndAbsoluteEncoderSubsystem &intake_back() const {
+    return intake_back_;
+  }
+  inline const PotAndAbsoluteEncoderSubsystem &turret() const {
+    return turret_;
+  }
+  inline const RelativeEncoderSubsystem &climber() const { return climber_; }
+
+  double robot_velocity() const;
+
  protected:
   virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
                             aos::Sender<Output>::Builder *output,
@@ -31,6 +51,12 @@
 
  private:
   RelativeEncoderSubsystem climber_;
+  PotAndAbsoluteEncoderSubsystem intake_front_;
+  PotAndAbsoluteEncoderSubsystem intake_back_;
+  PotAndAbsoluteEncoderSubsystem turret_;
+
+  aos::Fetcher<frc971::control_loops::drivetrain::Status>
+      drivetrain_status_fetcher_;
 
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };