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.h b/y2024/control_loops/superstructure/collision_avoidance.h
index fe36090..be58b35 100644
--- a/y2024/control_loops/superstructure/collision_avoidance.h
+++ b/y2024/control_loops/superstructure/collision_avoidance.h
@@ -23,10 +23,12 @@
struct Status {
double intake_pivot_position;
double turret_position;
+ double extend_position;
bool operator==(const Status &s) const {
return (intake_pivot_position == s.intake_pivot_position &&
- turret_position == s.turret_position);
+ turret_position == s.turret_position &&
+ extend_position == s.extend_position);
}
bool operator!=(const Status &s) const { return !(*this == s); }
};
@@ -35,32 +37,40 @@
static constexpr double kMinCollisionZoneTurret = 0.15;
static constexpr double kMaxCollisionZoneTurret = 1.15;
+ static constexpr double kSafeTurretExtendedPosition = 0.0;
+
// Maximum position of the intake to avoid collisions
static constexpr double kCollisionZoneIntake = 1.6;
+ static constexpr double kMinCollisionZoneExtend = 0.03;
+
// Tolerances for the subsystems
static constexpr double kEpsTurret = 0.05;
static constexpr double kEpsIntake = 0.05;
+ static constexpr double kEpsExtend = 0.01;
CollisionAvoidance();
// Reports if the superstructure is collided.
bool IsCollided(const Status &status);
- // Checks if there is a collision on either intake.
+ // Checks if there is a collision with the intake.
bool TurretCollided(double intake_position, double turret_position,
- double min_turret_collision_position,
- double max_turret_collision_position);
+ double extend_position);
+ // Checks if there is a collision with the extend.
+ bool ExtendCollided(double intake_position, double turret_position,
+ double extend_position);
// Checks and alters goals to make sure they're safe.
void UpdateGoal(const CollisionAvoidance::Status &status,
- const double &turret_goal_position);
+ double turret_goal_position, double extend_goal_position);
// Limits if goal is in collision spots.
- void CalculateAvoidance(bool intake_pivot, double intake_position,
- double turret_position, double turret_goal,
- double min_turret_collision_goal,
- double max_turret_collision_goal);
+ void CalculateAvoidance(double intake_position, double turret_position,
+ double extend_position, double turret_goal,
+ double extend_goal);
// Returns the goals to give to the respective control loops in
// superstructure.
+ double min_extend_goal() const { return min_extend_goal_; }
+ double max_extend_goal() const { return max_extend_goal_; }
double min_turret_goal() const { return min_turret_goal_; }
double max_turret_goal() const { return max_turret_goal_; }
double min_intake_pivot_goal() const { return min_intake_pivot_goal_; }
@@ -80,6 +90,12 @@
min_intake_pivot_goal_ =
::std::max(min_intake_pivot_goal, min_intake_pivot_goal_);
}
+ void update_min_extend_goal(double min_extend_goal) {
+ min_extend_goal_ = ::std::max(min_extend_goal, min_extend_goal_);
+ }
+ void update_max_extend_goal(double max_extend_goal) {
+ max_extend_goal_ = ::std::min(max_extend_goal, max_extend_goal_);
+ }
private:
void clear_min_intake_pivot_goal() {
@@ -94,11 +110,19 @@
void clear_max_turret_goal() {
max_turret_goal_ = ::std::numeric_limits<double>::infinity();
}
+ void clear_min_extend_goal() {
+ min_extend_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_extend_goal() {
+ max_extend_goal_ = ::std::numeric_limits<double>::infinity();
+ }
double min_intake_pivot_goal_;
double max_intake_pivot_goal_;
double min_turret_goal_;
double max_turret_goal_;
+ double min_extend_goal_;
+ double max_extend_goal_;
};
} // namespace y2024::control_loops::superstructure