Flip intake direction in collision avoidance
It makes much more sense that 0 is when it's fully down, and positive
is moving up.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I7419c7c942ca99ccdba4bf6e61bad174b67c0cc9
diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
index b39b48f..01b0463 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -58,7 +58,7 @@
if (turret_position_wrapped >= min_turret_collision_position &&
turret_position_wrapped <= max_turret_collision_position) {
// Reterns true if the intake is raised.
- if (intake_position <= kCollisionZoneIntake) {
+ if (intake_position > kCollisionZoneIntake) {
return true;
}
} else {
@@ -144,14 +144,14 @@
if (turret_pos_unsafe || turret_moving_past_intake) {
// If the turret is unsafe, limit the intake
if (intake_front) {
- update_min_intake_front_goal(kCollisionZoneIntake + kEpsIntake);
+ update_max_intake_front_goal(kCollisionZoneIntake - kEpsIntake);
} else {
- update_min_intake_back_goal(kCollisionZoneIntake + kEpsIntake);
+ update_max_intake_back_goal(kCollisionZoneIntake - kEpsIntake);
}
// If the intake is in the way, limit the turret until moved. Otherwise,
// let'errip!
- if (!turret_pos_unsafe && (intake_position <= kCollisionZoneIntake)) {
+ if (!turret_pos_unsafe && (intake_position > kCollisionZoneIntake)) {
if (turret_position < min_turret_collision_goal_unwrapped) {
update_max_turret_goal(min_turret_collision_goal_unwrapped -
kEpsTurret);