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>
diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
index 5d0fe27..b39b48f 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -67,8 +67,10 @@
return false;
}
-void CollisionAvoidance::UpdateGoal(const CollisionAvoidance::Status &status,
- const Goal *unsafe_goal) {
+void CollisionAvoidance::UpdateGoal(
+ const CollisionAvoidance::Status &status,
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *unsafe_turret_goal) {
// Start with our constraints being wide open.
clear_max_turret_goal();
clear_min_turret_goal();
@@ -81,10 +83,9 @@
const double intake_back_position = status.intake_back_position;
const double turret_position = status.turret_position;
- const double turret_goal =
- (unsafe_goal != nullptr && unsafe_goal->turret() != nullptr
- ? unsafe_goal->turret()->unsafe_goal()
- : std::numeric_limits<double>::quiet_NaN());
+ const double turret_goal = (unsafe_turret_goal != nullptr
+ ? unsafe_turret_goal->unsafe_goal()
+ : std::numeric_limits<double>::quiet_NaN());
// Calculating the avoidance with either intake, and when the turret is
// wrapped.