Add extend collision avoidance
If extend wants to go up, move turret to 0 and lock it out.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I13101a0c10d430436e765250b70a1257601680b2
diff --git a/y2024/control_loops/superstructure/collision_avoidance.cc b/y2024/control_loops/superstructure/collision_avoidance.cc
index 0594af0..bd82fe0 100644
--- a/y2024/control_loops/superstructure/collision_avoidance.cc
+++ b/y2024/control_loops/superstructure/collision_avoidance.cc
@@ -12,12 +12,14 @@
clear_max_intake_pivot_goal();
clear_min_turret_goal();
clear_max_turret_goal();
+ clear_min_extend_goal();
+ clear_max_extend_goal();
}
bool CollisionAvoidance::IsCollided(const CollisionAvoidance::Status &status) {
// Checks if intake front is collided.
if (TurretCollided(status.intake_pivot_position, status.turret_position,
- kMinCollisionZoneTurret, kMaxCollisionZoneTurret)) {
+ status.extend_position)) {
return true;
}
@@ -32,24 +34,39 @@
bool CollisionAvoidance::TurretCollided(double intake_position,
double turret_position,
- double min_turret_collision_position,
- double max_turret_collision_position) {
+ double extend_position) {
// Checks if turret is in the collision area.
- if (AngleInRange(turret_position, min_turret_collision_position,
- max_turret_collision_position)) {
+ if (AngleInRange(turret_position, kMinCollisionZoneTurret,
+ kMaxCollisionZoneTurret)) {
// Returns true if the intake is raised.
if (intake_position > kCollisionZoneIntake) {
return true;
}
- } else {
- return false;
}
+ return ExtendCollided(intake_position, turret_position, extend_position);
+}
+
+bool CollisionAvoidance::ExtendCollided(double /*intake_position*/,
+ double turret_position,
+ double extend_position) {
+ // Checks if turret is in the collision area.
+ if (!AngleInRange(turret_position, kSafeTurretExtendedPosition - kEpsTurret,
+ kSafeTurretExtendedPosition + kEpsTurret)) {
+ // Returns true if the extend is raised.
+ if (extend_position > kMinCollisionZoneExtend) {
+ return true;
+ }
+ }
+
return false;
}
void CollisionAvoidance::UpdateGoal(const CollisionAvoidance::Status &status,
- const double &turret_goal_position) {
+ const double turret_goal_position,
+ const double extend_goal_position) {
// Start with our constraints being wide open.
+ clear_min_extend_goal();
+ clear_max_extend_goal();
clear_max_turret_goal();
clear_min_turret_goal();
clear_max_intake_pivot_goal();
@@ -57,54 +74,83 @@
const double intake_pivot_position = status.intake_pivot_position;
const double turret_position = status.turret_position;
+ const double extend_position = status.extend_position;
const double turret_goal = turret_goal_position;
+ const double extend_goal = extend_goal_position;
// Calculating the avoidance with the intake
- CalculateAvoidance(true, intake_pivot_position, turret_position, turret_goal,
- kMinCollisionZoneTurret, kMaxCollisionZoneTurret);
+ CalculateAvoidance(intake_pivot_position, turret_position, extend_position,
+ turret_goal, extend_goal);
}
-void CollisionAvoidance::CalculateAvoidance(bool intake_pivot,
- double intake_position,
+void CollisionAvoidance::CalculateAvoidance(double intake_position,
double turret_position,
+ double extend_position,
double turret_goal,
- double min_turret_collision_goal,
- double max_turret_collision_goal) {
+ double extend_goal) {
// If the turret goal is in a collison zone or moving through one, limit
// intake.
- const bool turret_pos_unsafe = AngleInRange(
- turret_position, min_turret_collision_goal, max_turret_collision_goal);
+ const bool turret_intake_pos_unsafe = AngleInRange(
+ turret_position, kMinCollisionZoneTurret, kMaxCollisionZoneTurret);
+ const bool turret_extend_pos_unsafe =
+ turret_position > kEpsTurret + kSafeTurretExtendedPosition ||
+ turret_position < -kEpsTurret + kSafeTurretExtendedPosition;
+
+ const bool extend_goal_unsafe =
+ extend_goal > kMinCollisionZoneExtend - kEpsExtend;
+ const bool extend_position_unsafe =
+ extend_position > kMinCollisionZoneExtend - kEpsExtend;
+
+ // OK, we are trying to move the extend, and need the turret to be at 0.
+ // Pretend that's the goal.
+ if (extend_goal_unsafe || extend_position_unsafe) {
+ turret_goal = kSafeTurretExtendedPosition;
+ }
const bool turret_moving_forward = (turret_goal > turret_position);
// Check if the closest angles are going to be passed
const bool turret_moving_past_intake =
- ((turret_moving_forward &&
- (turret_position <= max_turret_collision_goal &&
- turret_goal >= min_turret_collision_goal)) ||
- (!turret_moving_forward &&
- (turret_position >= min_turret_collision_goal &&
- turret_goal <= max_turret_collision_goal)));
+ ((turret_moving_forward && (turret_position <= kMaxCollisionZoneTurret &&
+ turret_goal >= kMinCollisionZoneTurret)) ||
+ (!turret_moving_forward && (turret_position >= kMinCollisionZoneTurret &&
+ turret_goal <= kMaxCollisionZoneTurret)));
- if (turret_pos_unsafe || turret_moving_past_intake) {
+ if (turret_intake_pos_unsafe || turret_moving_past_intake) {
// If the turret is unsafe, limit the intake
- if (intake_pivot) {
- update_max_intake_pivot_goal(kCollisionZoneIntake - kEpsIntake);
- }
+ update_max_intake_pivot_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_intake_pos_unsafe && (intake_position > kCollisionZoneIntake)) {
if (turret_position <
- (min_turret_collision_goal + max_turret_collision_goal / 2)) {
- update_max_turret_goal(min_turret_collision_goal - kEpsTurret);
+ (kMinCollisionZoneTurret + kMaxCollisionZoneTurret) / 2.) {
+ update_max_turret_goal(kMinCollisionZoneTurret - kEpsTurret);
} else {
- update_min_turret_goal(max_turret_collision_goal + kEpsTurret);
+ update_min_turret_goal(kMaxCollisionZoneTurret + kEpsTurret);
}
}
}
+
+ // OK, the logic is pretty simple. The turret needs to be at
+ // kSafeTurretExtendedPosition any time extend is > kMinCollisionZoneExtend.
+ //
+ // Extend can't go up if the turret isn't near 0.
+ if (turret_extend_pos_unsafe) {
+ update_max_extend_goal(kMinCollisionZoneExtend - kEpsExtend);
+ }
+
+ // Turret is bound to the safe position if extend wants to be, or is unsafe.
+ if (extend_goal_unsafe || extend_position_unsafe) {
+ // If the turret isn't allowed to go to 0, don't drive it there.
+ if (min_turret_goal() < kSafeTurretExtendedPosition &&
+ max_turret_goal() > kSafeTurretExtendedPosition) {
+ update_min_turret_goal(kSafeTurretExtendedPosition);
+ update_max_turret_goal(kSafeTurretExtendedPosition);
+ }
+ }
}
} // namespace y2024::control_loops::superstructure