commit 7a79960b5ec2d61047760dd4f8a8672000e4fc81
author Filip Kujawa <filip.j.kujawa@gmail.com> Fri Feb 23 12:27:47 2024 -0800
committer Filip Kujawa <filip.j.kujawa@gmail.com> Sat Feb 24 17:17:26 2024 -0800
tree 9e6725176f3faf9aa1a6156a8d785a0931d0344c
parent 3d43951badb902767148022c60fc5e920fa20be9

Add Extend Superstructure and State Machine

Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: Id35e2156499384502275af306ff7042c7800d31f

y2024/control_loops/superstructure/superstructure.h

diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 650092b..963d8c4 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -3,6 +3,7 @@
 
 #include "aos/events/event_loop.h"
 #include "aos/json_to_flatbuffer.h"
+#include "aos/time/time.h"
 #include "frc971/constants/constants_sender_lib.h"
 #include "frc971/control_loops/control_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
@@ -33,7 +34,6 @@
           ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
 
   explicit Superstructure(::aos::EventLoop *event_loop,
-                          std::shared_ptr<const constants::Values> values,
                           const ::std::string &name = "/superstructure");
 
   inline const AbsoluteEncoderSubsystem &intake_pivot() const {
@@ -45,6 +45,9 @@
   }
 
   inline const Shooter &shooter() const { return shooter_; }
+  inline const PotAndAbsoluteEncoderSubsystem &extend() const {
+    return extend_;
+  }
 
   double robot_velocity() const;
 
@@ -54,7 +57,6 @@
                             aos::Sender<Status>::Builder *status) override;
 
  private:
-  std::shared_ptr<const constants::Values> values_;
   frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
   const Constants *robot_constants_;
   aos::Fetcher<frc971::control_loops::drivetrain::Status>
@@ -65,12 +67,19 @@
 
   aos::Alliance alliance_ = aos::Alliance::kInvalid;
 
-  TransferRollerGoal transfer_goal_;
+  bool catapult_requested_ = false;
+
+  SuperstructureState state_ = SuperstructureState::IDLE;
+
+  aos::monotonic_clock::time_point transfer_start_time_ =
+      aos::monotonic_clock::time_point::min();
+
   AbsoluteEncoderSubsystem intake_pivot_;
   PotAndAbsoluteEncoderSubsystem climber_;
 
   Shooter shooter_;
 
+  PotAndAbsoluteEncoderSubsystem extend_;
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
 };