Add collision avoidance
Makes sure that the intakes dont collide with the back of the catapult
Signed-off-by: Henry Speiser <henry@speiser.net>
Change-Id: Ie25dd244d2ef316bd1c3dfa20636c5b6150113ef
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index da1af82..bf94b08 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -1,6 +1,7 @@
#include "y2022/control_loops/superstructure/superstructure.h"
#include "aos/events/event_loop.h"
+#include "y2022/control_loops/superstructure/collision_avoidance.h"
namespace y2022 {
namespace control_loops {
@@ -42,6 +43,19 @@
catapult_.Reset();
}
+ collision_avoidance_.UpdateGoal(
+ {.intake_front_position = intake_front_.estimated_position(),
+ .intake_back_position = intake_back_.estimated_position(),
+ .turret_position = turret_.estimated_position()},
+ unsafe_goal);
+
+ turret_.set_min_position(collision_avoidance_.min_turret_goal());
+ turret_.set_max_position(collision_avoidance_.max_turret_goal());
+ intake_front_.set_min_position(collision_avoidance_.min_intake_front_goal());
+ intake_front_.set_max_position(collision_avoidance_.max_intake_front_goal());
+ intake_back_.set_min_position(collision_avoidance_.min_intake_back_goal());
+ intake_back_.set_max_position(collision_avoidance_.max_intake_back_goal());
+
drivetrain_status_fetcher_.Fetch();
const float velocity = robot_velocity();