commit d51af7a6af76ff8adef0f5168cdd5d8da4d10080
author Ravago Jones <ravagojones@gmail.com> Sat Mar 26 21:44:20 2022 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Apr 13 19:33:05 2022 -0700
tree 81ca94c350ec80e219280c659e990cf9808498b8
parent 0949c04b8bde5a3205ff98e9f77a678957fa0a7c

Add ball sorting

Discard balls that are not our alliance color

Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: Ic9ff9bfc724c74d0c6de4364ddc3999945c062d2

y2022/control_loops/superstructure/superstructure.h

diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 14fa8ab..48d07f1 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -13,6 +13,7 @@
 #include "y2022/control_loops/superstructure/superstructure_position_generated.h"
 #include "y2022/control_loops/superstructure/superstructure_status_generated.h"
 #include "y2022/control_loops/superstructure/turret/aiming.h"
+#include "y2022/vision/ball_color_generated.h"
 
 namespace y2022 {
 namespace control_loops {
@@ -35,6 +36,8 @@
   static constexpr double kCatapultGoalThreshold = 0.05;
   // potentiometer will be more noisy
   static constexpr double kFlipperGoalThreshold = 0.05;
+  static constexpr double kDiscardingPosition = 0.35;
+  static constexpr double kDiscardingVelocity = 6.0;
 
   explicit Superstructure(::aos::EventLoop *event_loop,
                           std::shared_ptr<const constants::Values> values,
@@ -72,6 +75,8 @@
   aos::Fetcher<frc971::control_loops::drivetrain::Status>
       drivetrain_status_fetcher_;
   aos::Fetcher<CANPosition> can_position_fetcher_;
+  aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+  aos::Fetcher<y2022::vision::BallColor> ball_color_fetcher_;
 
   int prev_shot_count_ = 0;
 
@@ -80,6 +85,9 @@
   bool flippers_open_ = false;
   bool reseating_in_catapult_ = false;
   bool fire_ = false;
+  bool discarding_ball_ = false;
+  aos::Alliance alliance_ = aos::Alliance::kInvalid;
+  aos::Alliance ball_color_ = aos::Alliance::kInvalid;
 
   aos::monotonic_clock::time_point front_intake_beambreak_timer_ =
       aos::monotonic_clock::min_time;