commit 5da0635d23e44c89259297a108c440164dcc8b65
author Ravago Jones <ravagojones@gmail.com> Fri Mar 04 20:26:24 2022 -0800
committer Ravago Jones <ravagojones@gmail.com> Fri Mar 04 20:26:24 2022 -0800
tree 463036ec0a0a0b1f1a329884560424bf5de17190
parent 20b9638eb25b2cbf077d31ef3f27c0e435052e5e

Add superstructure state machine

Managing balls from the intake into the catapult

Change-Id: I88535ee82a876d63fe49e3607c6986690d704daf
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Signed-off-by: Ravago Jones <ravagojones@gmail.com>

y2022/control_loops/superstructure/superstructure.h

diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index dfd4265..13a790a 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -7,6 +7,7 @@
 #include "y2022/constants.h"
 #include "y2022/control_loops/superstructure/catapult/catapult.h"
 #include "y2022/control_loops/superstructure/collision_avoidance.h"
+#include "y2022/control_loops/superstructure/superstructure_can_position_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_output_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_position_generated.h"
@@ -29,6 +30,11 @@
           ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
           ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
 
+  static constexpr double kTurretGoalThreshold = 0.01;
+  static constexpr double kCatapultGoalThreshold = 0.01;
+  // potentiometer will be more noisy
+  static constexpr double kFlipperGoalThreshold = 0.05;
+
   explicit Superstructure(::aos::EventLoop *event_loop,
                           std::shared_ptr<const constants::Values> values,
                           const ::std::string &name = "/superstructure");
@@ -58,15 +64,32 @@
   PotAndAbsoluteEncoderSubsystem intake_front_;
   PotAndAbsoluteEncoderSubsystem intake_back_;
   PotAndAbsoluteEncoderSubsystem turret_;
+  catapult::Catapult catapult_;
 
   CollisionAvoidance collision_avoidance_;
 
   aos::Fetcher<frc971::control_loops::drivetrain::Status>
       drivetrain_status_fetcher_;
+  aos::Fetcher<CANPosition> can_position_fetcher_;
+
+  int prev_shot_count_ = 0;
+
+  bool flippers_open_ = false;
+  bool reseating_in_catapult_ = false;
+  bool fire_ = false;
+
+  aos::monotonic_clock::time_point intake_beambreak_timer_ =
+      aos::monotonic_clock::min_time;
+  aos::monotonic_clock::time_point transferring_timer_ =
+      aos::monotonic_clock::min_time;
+  aos::monotonic_clock::time_point loading_timer_ =
+      aos::monotonic_clock::min_time;
+  aos::monotonic_clock::time_point flipper_opening_start_time_ =
+      aos::monotonic_clock::min_time;
+  SuperstructureState state_ = SuperstructureState::IDLE;
+  IntakeState intake_state_ = IntakeState::NO_BALL;
 
   DISALLOW_COPY_AND_ASSIGN(Superstructure);
-
-  catapult::Catapult catapult_;
 };
 
 }  // namespace superstructure