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/BUILD b/y2022/control_loops/superstructure/BUILD
index f08bb80..73f7618 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -68,6 +68,7 @@
"superstructure.h",
],
deps = [
+ ":collision_avoidance_lib",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
@@ -118,6 +119,35 @@
],
)
+cc_library(
+ name = "collision_avoidance_lib",
+ srcs = ["collision_avoidance.cc"],
+ hdrs = ["collision_avoidance.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/functional:bind_front",
+ ],
+)
+
+cc_test(
+ name = "collision_avoidance_test",
+ srcs = ["collision_avoidance_test.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":collision_avoidance_lib",
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
+ "//aos:flatbuffers",
+ "//aos:math",
+ "//aos/testing:googletest",
+ ],
+)
+
ts_library(
name = "superstructure_plotter",
srcs = ["superstructure_plotter.ts"],
diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
new file mode 100644
index 0000000..5d0fe27
--- /dev/null
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -0,0 +1,167 @@
+#include "y2022/control_loops/superstructure/collision_avoidance.h"
+
+#include <cmath>
+
+#include "absl/functional/bind_front.h"
+#include "glog/logging.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+
+CollisionAvoidance::CollisionAvoidance() {
+ clear_min_intake_front_goal();
+ clear_max_intake_front_goal();
+ clear_min_intake_back_goal();
+ clear_max_intake_back_goal();
+ clear_min_turret_goal();
+ clear_max_turret_goal();
+}
+
+bool CollisionAvoidance::IsCollided(const CollisionAvoidance::Status &status) {
+ // Checks if intake front is collided.
+ if (TurretCollided(status.intake_front_position, status.turret_position,
+ kMinCollisionZoneFrontTurret,
+ kMaxCollisionZoneFrontTurret)) {
+ return true;
+ }
+
+ // Checks if intake back is collided.
+ if (TurretCollided(status.intake_back_position, status.turret_position,
+ kMinCollisionZoneBackTurret,
+ kMaxCollisionZoneBackTurret)) {
+ return true;
+ }
+
+ return false;
+}
+
+std::pair<double, int> WrapTurretAngle(double turret_angle) {
+ double wrapped = std::remainder(turret_angle - M_PI, 2 * M_PI) + M_PI;
+ int wraps =
+ static_cast<int>(std::round((turret_angle - wrapped) / (2 * M_PI)));
+ return {wrapped, wraps};
+}
+
+double UnwrapTurretAngle(double wrapped, int wraps) {
+ return wrapped + 2.0 * M_PI * wraps;
+}
+
+bool CollisionAvoidance::TurretCollided(double intake_position,
+ double turret_position,
+ double min_turret_collision_position,
+ double max_turret_collision_position) {
+ const auto turret_position_wrapped_pair = WrapTurretAngle(turret_position);
+ const double turret_position_wrapped = turret_position_wrapped_pair.first;
+
+ // Checks if turret is in the collision area.
+ 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) {
+ return true;
+ }
+ } else {
+ return false;
+ }
+ return false;
+}
+
+void CollisionAvoidance::UpdateGoal(const CollisionAvoidance::Status &status,
+ const Goal *unsafe_goal) {
+ // Start with our constraints being wide open.
+ clear_max_turret_goal();
+ clear_min_turret_goal();
+ clear_max_intake_front_goal();
+ clear_min_intake_front_goal();
+ clear_max_intake_back_goal();
+ clear_min_intake_back_goal();
+
+ const double intake_front_position = status.intake_front_position;
+ 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());
+
+ // Calculating the avoidance with either intake, and when the turret is
+ // wrapped.
+
+ CalculateAvoidance(true, intake_front_position, turret_goal,
+ kMinCollisionZoneFrontTurret, kMaxCollisionZoneFrontTurret,
+ turret_position);
+ CalculateAvoidance(false, intake_back_position, turret_goal,
+ kMinCollisionZoneBackTurret, kMaxCollisionZoneBackTurret,
+ turret_position);
+}
+
+void CollisionAvoidance::CalculateAvoidance(bool intake_front,
+ double intake_position,
+ double turret_goal,
+ double min_turret_collision_goal,
+ double max_turret_collision_goal,
+ double turret_position) {
+ auto [turret_position_wrapped, turret_position_wraps] =
+ WrapTurretAngle(turret_position);
+
+ // If the turret goal is in a collison zone or moving through one, limit
+ // intake.
+ const bool turret_pos_unsafe =
+ (turret_position_wrapped >= min_turret_collision_goal &&
+ turret_position_wrapped <= max_turret_collision_goal);
+
+ const bool turret_moving_forward = (turret_goal > turret_position);
+
+ // To figure out if we are moving past an intake, find the unwrapped min/max
+ // angles closest to the turret position on the journey.
+ int bounds_wraps = turret_position_wraps;
+ double min_turret_collision_goal_unwrapped =
+ UnwrapTurretAngle(min_turret_collision_goal, bounds_wraps);
+ if (turret_moving_forward &&
+ min_turret_collision_goal_unwrapped < turret_position) {
+ bounds_wraps++;
+ } else if (!turret_moving_forward &&
+ min_turret_collision_goal_unwrapped > turret_position) {
+ bounds_wraps--;
+ }
+ min_turret_collision_goal_unwrapped =
+ UnwrapTurretAngle(min_turret_collision_goal, bounds_wraps);
+ const double max_turret_collision_goal_unwrapped =
+ UnwrapTurretAngle(max_turret_collision_goal, bounds_wraps);
+
+ // Check if the closest unwrapped angles are going to be passed
+ const bool turret_moving_past_intake =
+ ((turret_moving_forward &&
+ (turret_position <= max_turret_collision_goal_unwrapped &&
+ turret_goal >= min_turret_collision_goal_unwrapped)) ||
+ (!turret_moving_forward &&
+ (turret_position >= min_turret_collision_goal_unwrapped &&
+ turret_goal <= max_turret_collision_goal_unwrapped)));
+
+ 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);
+ } else {
+ update_min_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_position < min_turret_collision_goal_unwrapped) {
+ update_max_turret_goal(min_turret_collision_goal_unwrapped -
+ kEpsTurret);
+ } else {
+ update_min_turret_goal(max_turret_collision_goal_unwrapped +
+ kEpsTurret);
+ }
+ }
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
diff --git a/y2022/control_loops/superstructure/collision_avoidance.h b/y2022/control_loops/superstructure/collision_avoidance.h
new file mode 100644
index 0000000..94b454c
--- /dev/null
+++ b/y2022/control_loops/superstructure/collision_avoidance.h
@@ -0,0 +1,143 @@
+#ifndef Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDENCE_H_
+#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDENCE_H_
+
+#include <cmath>
+
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+
+// Returns the wrapped angle as well as number of wraps (positive or negative).
+// The returned angle will be inside [0.0, 2 * M_PI).
+std::pair<double, int> WrapTurretAngle(double turret_angle);
+
+// Returns the absolute angle given the wrapped angle and number of wraps.
+double UnwrapTurretAngle(double wrapped, int wraps);
+
+// 1. Prevent the turret from moving if the intake is up
+// 2. If the intake is up, drop it so it is not in the way
+// 3. Move the turret to the desired position.
+// 4. When the turret moves away, if the intake is down, move it back up.
+class CollisionAvoidance {
+ public:
+ struct Status {
+ double intake_front_position;
+ double intake_back_position;
+ double turret_position;
+
+ bool operator==(const Status &s) const {
+ return (intake_front_position == s.intake_front_position &&
+ intake_back_position == s.intake_back_position &&
+ turret_position == s.turret_position);
+ }
+ bool operator!=(const Status &s) const { return !(*this == s); }
+ };
+
+ // TODO(henry): put actual constants here.
+
+ // Reference angles between which the turret will be careful
+ static constexpr double kCollisionZoneTurret = M_PI * 5.0 / 18.0;
+
+ // For the turret, 0 rad is pointing straight forwards
+ static constexpr double kMinCollisionZoneFrontTurret =
+ M_PI - kCollisionZoneTurret;
+ static constexpr double kMaxCollisionZoneFrontTurret =
+ M_PI + kCollisionZoneTurret;
+
+ static constexpr double kMinCollisionZoneBackTurret = -kCollisionZoneTurret;
+ static constexpr double kMaxCollisionZoneBackTurret = kCollisionZoneTurret;
+
+ // Minimum (highest in reality) of the intake, in order to avoid collisions
+ static constexpr double kCollisionZoneIntake = M_PI / 6.0;
+
+ // Tolerance for the turret.
+ static constexpr double kEpsTurret = 0.05;
+ // Tolerance for the intake.
+ static constexpr double kEpsIntake = 0.05;
+
+ CollisionAvoidance();
+
+ // Reports if the superstructure is collided.
+ bool IsCollided(const Status &status);
+ // Checks if there is a collision on either intake.
+ bool TurretCollided(double intake_position, double turret_position,
+ double min_turret_collision_position,
+ double max_turret_collision_position);
+ // Checks and alters goals to make sure they're safe.
+ void UpdateGoal(const Status &status, const Goal *unsafe_goal);
+ // Limits if goal is in collision spots.
+ void CalculateAvoidance(bool intake_front, double intake_position,
+ double turret_goal, double mix_turret_collision_goal,
+ double max_turret_collision_goal,
+ double turret_position);
+
+ // Returns the goals to give to the respective control loops in
+ // superstructure.
+ double min_turret_goal() const { return min_turret_goal_; }
+ double max_turret_goal() const { return max_turret_goal_; }
+ double min_intake_front_goal() const { return min_intake_front_goal_; }
+ double max_intake_front_goal() const { return max_intake_front_goal_; }
+ double min_intake_back_goal() const { return min_intake_back_goal_; }
+ double max_intake_back_goal() const { return max_intake_back_goal_; }
+
+ void update_max_turret_goal(double max_turret_goal) {
+ max_turret_goal_ = ::std::min(max_turret_goal, max_turret_goal_);
+ }
+ void update_min_turret_goal(double min_turret_goal) {
+ min_turret_goal_ = ::std::max(min_turret_goal, min_turret_goal_);
+ }
+ void update_max_intake_front_goal(double max_intake_front_goal) {
+ max_intake_front_goal_ =
+ ::std::min(max_intake_front_goal, max_intake_front_goal_);
+ }
+ void update_min_intake_front_goal(double min_intake_front_goal) {
+ min_intake_front_goal_ =
+ ::std::max(min_intake_front_goal, min_intake_front_goal_);
+ }
+ void update_max_intake_back_goal(double max_intake_back_goal) {
+ max_intake_back_goal_ =
+ ::std::min(max_intake_back_goal, max_intake_back_goal_);
+ }
+ void update_min_intake_back_goal(double min_intake_back_goal) {
+ min_intake_back_goal_ =
+ ::std::max(min_intake_back_goal, min_intake_back_goal_);
+ }
+
+ private:
+ void clear_min_intake_front_goal() {
+ min_intake_front_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_intake_front_goal() {
+ max_intake_front_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_intake_back_goal() {
+ min_intake_back_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_intake_back_goal() {
+ max_intake_back_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_turret_goal() {
+ min_turret_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_turret_goal() {
+ max_turret_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+
+ double min_intake_front_goal_;
+ double max_intake_front_goal_;
+ double min_intake_back_goal_;
+ double max_intake_back_goal_;
+ double min_turret_goal_;
+ double max_turret_goal_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
+
+#endif
diff --git a/y2022/control_loops/superstructure/collision_avoidance_test.cc b/y2022/control_loops/superstructure/collision_avoidance_test.cc
new file mode 100644
index 0000000..616a10b
--- /dev/null
+++ b/y2022/control_loops/superstructure/collision_avoidance_test.cc
@@ -0,0 +1,423 @@
+#include "y2022/control_loops/superstructure/collision_avoidance.h"
+
+#include "aos/commonmath.h"
+#include "aos/flatbuffers.h"
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2022::control_loops::superstructure::testing {
+
+using aos::FlatbufferDetachedBuffer;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+FlatbufferDetachedBuffer<Goal> MakeZeroGoal() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_front_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_front_builder(
+ fbb);
+
+ intake_front_builder.add_unsafe_goal(0.0);
+ intake_front_offset = intake_front_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_back_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_back_builder(
+ fbb);
+
+ intake_back_builder.add_unsafe_goal(0.0);
+ intake_back_offset = intake_back_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder turret_builder(fbb);
+
+ turret_builder.add_unsafe_goal(0.0);
+ turret_offset = turret_builder.Finish();
+ }
+
+ superstructure::Goal::Builder goal_builder(fbb);
+
+ goal_builder.add_intake_front(intake_front_offset);
+ goal_builder.add_intake_back(intake_back_offset);
+ goal_builder.add_turret(turret_offset);
+
+ fbb.Finish(goal_builder.Finish());
+ return fbb.Release();
+}
+
+// Enums for the different classes of intake and turret states
+enum class IntakeState { kSafe, kUnsafe };
+enum class TurretState {
+ kSafeFront,
+ kSafeBack,
+ kSafeFrontWrapped,
+ kSafeBackWrapped,
+ kUnsafeFront,
+ kUnsafeBack,
+ kUnsafeFrontWrapped,
+ kUnsafeBackWrapped,
+ kNegativeSafeFront,
+ kNegativeSafeBack,
+ kNegativeSafeFrontWrapped,
+ kNegativeSafeBackWrapped,
+ kNegativeUnsafeFront,
+ kNegativeUnsafeBack,
+ kNegativeUnsafeFrontWrapped,
+ kNegativeUnsafeBackWrapped,
+};
+
+class CollisionAvoidanceTest : public ::testing::Test {
+ public:
+ CollisionAvoidanceTest()
+ : unsafe_goal_(MakeZeroGoal()),
+ status_({0.0, 0.0, 0.0}),
+ prev_status_({0.0, 0.0, 0.0}) {}
+
+ int t = 0;
+ int d = 0;
+ void Simulate() {
+ FlatbufferDetachedBuffer<Goal> safe_goal = MakeZeroGoal();
+
+ t++;
+ // Don't simulate if already collided
+ if (avoidance_.IsCollided(status_)) {
+ d++;
+ return;
+ }
+
+ bool moving = true;
+ while (moving) {
+ // Compute the safe goal
+ avoidance_.UpdateGoal(status_, &unsafe_goal_.message());
+
+ // The system should never be collided
+ ASSERT_FALSE(avoidance_.IsCollided(status_));
+
+ safe_goal.mutable_message()->mutable_intake_front()->mutate_unsafe_goal(
+ ::aos::Clip(intake_front_goal(), avoidance_.min_intake_front_goal(),
+ avoidance_.max_intake_front_goal()));
+ safe_goal.mutable_message()->mutable_intake_back()->mutate_unsafe_goal(
+ ::aos::Clip(intake_back_goal(), avoidance_.min_intake_back_goal(),
+ avoidance_.max_intake_back_goal()));
+ safe_goal.mutable_message()->mutable_turret()->mutate_unsafe_goal(
+ ::aos::Clip(turret_goal(), avoidance_.min_turret_goal(),
+ avoidance_.max_turret_goal()));
+
+ // Move each subsystem towards their goals a bit
+ status_.intake_front_position =
+ LimitedMove(status_.intake_front_position,
+ safe_goal.message().intake_front()->unsafe_goal());
+ status_.intake_back_position =
+ LimitedMove(status_.intake_back_position,
+ safe_goal.message().intake_back()->unsafe_goal());
+ status_.turret_position = LimitedMove(
+ status_.turret_position, safe_goal.message().turret()->unsafe_goal());
+
+ // If it stopped moving, we're done
+ if (!IsMoving()) {
+ moving = false;
+ } else {
+ prev_status_ = status_;
+ }
+ }
+
+ CheckGoals();
+ }
+
+ bool IsMoving() { return (status_ != prev_status_); }
+
+ double ComputeIntakeAngle(IntakeState intake_state) {
+ double intake_angle = 0.0;
+
+ switch (intake_state) {
+ case IntakeState::kSafe:
+ intake_angle = CollisionAvoidance::kCollisionZoneIntake +
+ CollisionAvoidance::kEpsIntake;
+ break;
+ case IntakeState::kUnsafe:
+ intake_angle = CollisionAvoidance::kCollisionZoneIntake -
+ CollisionAvoidance::kEpsIntake;
+ break;
+ }
+
+ return intake_angle;
+ }
+
+ double ComputeTurretAngle(TurretState turret_state) {
+ double turret_angle = 0.0;
+
+ constexpr double kMaxCollisionZoneFrontTurretWrapped =
+ (2.0 * M_PI) + CollisionAvoidance::kMaxCollisionZoneFrontTurret;
+ constexpr double kMaxCollisionZoneBackTurretWrapped =
+ (2.0 * M_PI) + CollisionAvoidance::kMaxCollisionZoneBackTurret;
+
+ switch (turret_state) {
+ case TurretState::kSafeFront:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneFrontTurret +
+ CollisionAvoidance::kEpsTurret;
+ break;
+ case TurretState::kSafeBack:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneBackTurret +
+ CollisionAvoidance::kEpsTurret;
+ break;
+ case TurretState::kSafeFrontWrapped:
+ turret_angle = kMaxCollisionZoneFrontTurretWrapped +
+ CollisionAvoidance::kEpsTurret;
+ break;
+ case TurretState::kSafeBackWrapped:
+ turret_angle =
+ kMaxCollisionZoneBackTurretWrapped + CollisionAvoidance::kEpsTurret;
+ break;
+ case TurretState::kUnsafeFront:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneFrontTurret -
+ CollisionAvoidance::kEpsTurret;
+ break;
+ case TurretState::kUnsafeBack:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneBackTurret -
+ CollisionAvoidance::kEpsTurret;
+ break;
+ case TurretState::kUnsafeFrontWrapped:
+ turret_angle = kMaxCollisionZoneFrontTurretWrapped -
+ CollisionAvoidance::kEpsTurret;
+ break;
+ case TurretState::kUnsafeBackWrapped:
+ turret_angle =
+ kMaxCollisionZoneBackTurretWrapped - CollisionAvoidance::kEpsTurret;
+ break;
+
+ case TurretState::kNegativeSafeFront:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneFrontTurret +
+ CollisionAvoidance::kEpsTurret - 2.0 * M_PI;
+ break;
+ case TurretState::kNegativeSafeBack:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneBackTurret +
+ CollisionAvoidance::kEpsTurret - 2.0 * M_PI;
+ break;
+ case TurretState::kNegativeSafeFrontWrapped:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneFrontTurret +
+ CollisionAvoidance::kEpsTurret - 4.0 * M_PI;
+ break;
+ case TurretState::kNegativeSafeBackWrapped:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneBackTurret +
+ CollisionAvoidance::kEpsTurret - 4.0 * M_PI;
+ break;
+ case TurretState::kNegativeUnsafeFront:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneFrontTurret -
+ CollisionAvoidance::kEpsTurret - 2.0 * M_PI;
+ break;
+ case TurretState::kNegativeUnsafeBack:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneBackTurret -
+ CollisionAvoidance::kEpsTurret - 2.0 * M_PI;
+ break;
+ case TurretState::kNegativeUnsafeFrontWrapped:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneFrontTurret -
+ CollisionAvoidance::kEpsTurret - 4.0 * M_PI;
+ break;
+ case TurretState::kNegativeUnsafeBackWrapped:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneBackTurret -
+ CollisionAvoidance::kEpsTurret - 4.0 * M_PI;
+ break;
+ }
+
+ return turret_angle;
+ }
+
+ void Test(IntakeState intake_front_pos_state,
+ IntakeState intake_back_pos_state, TurretState turret_pos_state,
+ IntakeState intake_front_goal_state,
+ IntakeState intake_back_goal_state, TurretState turret_goal_state) {
+ status_ = {ComputeIntakeAngle(intake_front_pos_state),
+ ComputeIntakeAngle(intake_back_pos_state),
+ ComputeTurretAngle(turret_pos_state)};
+
+ unsafe_goal_.mutable_message()->mutable_intake_front()->mutate_unsafe_goal(
+ ComputeIntakeAngle(intake_front_goal_state));
+ unsafe_goal_.mutable_message()->mutable_intake_back()->mutate_unsafe_goal(
+ ComputeIntakeAngle(intake_back_goal_state));
+
+ unsafe_goal_.mutable_message()->mutable_turret()->mutate_unsafe_goal(
+ ComputeTurretAngle(turret_goal_state));
+
+ Simulate();
+ }
+
+ // Provide goals and status messages
+ FlatbufferDetachedBuffer<Goal> unsafe_goal_;
+ CollisionAvoidance::Status status_;
+
+ private:
+ static constexpr double kIterationMove = 0.001;
+
+ void CheckGoals() {
+ // Check to see if we reached the goals
+ // Turret is highest priority and should always reach the unsafe goal
+ EXPECT_NEAR(turret_goal(), status_.turret_position, kIterationMove);
+
+ // If the unsafe goal had an intake colliding with the turret, the intake
+ // position should be at least the collision zone angle.
+ // Otherwise, the intake should be at the unsafe goal
+ if (avoidance_.TurretCollided(
+ intake_front_goal(), turret_goal(),
+ CollisionAvoidance::kMinCollisionZoneFrontTurret,
+ CollisionAvoidance::kMaxCollisionZoneFrontTurret)) {
+ EXPECT_GE(status_.intake_front_position,
+ CollisionAvoidance::kCollisionZoneIntake);
+ } else {
+ EXPECT_NEAR(intake_front_goal(), status_.intake_front_position,
+ kIterationMove);
+ }
+
+ if (avoidance_.TurretCollided(
+ intake_back_goal(), turret_goal(),
+ CollisionAvoidance::kMinCollisionZoneBackTurret,
+ CollisionAvoidance::kMaxCollisionZoneBackTurret)) {
+ EXPECT_GE(status_.intake_back_position,
+ CollisionAvoidance::kCollisionZoneIntake);
+ } else {
+ EXPECT_NEAR(intake_back_goal(), status_.intake_back_position, 0.001);
+ }
+ }
+
+ double LimitedMove(double position, double goal) {
+ if (position + kIterationMove < goal) {
+ return position + kIterationMove;
+ } else if (position - kIterationMove > goal) {
+ return position - kIterationMove;
+ } else {
+ return goal;
+ }
+ }
+
+ double intake_front_goal() const {
+ return unsafe_goal_.message().intake_front()->unsafe_goal();
+ }
+ double intake_back_goal() const {
+ return unsafe_goal_.message().intake_back()->unsafe_goal();
+ }
+ double turret_goal() const {
+ return unsafe_goal_.message().turret()->unsafe_goal();
+ }
+
+ CollisionAvoidance avoidance_;
+ CollisionAvoidance::Status prev_status_;
+};
+
+// Just to be safe, brute force ALL the possible position-goal combinations
+// and make sure we never collide and the correct goals are reached
+TEST_F(CollisionAvoidanceTest, BruteForce) {
+ // Intake front position
+ for (IntakeState intake_front_pos :
+ {IntakeState::kSafe, IntakeState::kUnsafe}) {
+ // Intake back position
+ for (IntakeState intake_back_pos :
+ {IntakeState::kSafe, IntakeState::kUnsafe}) {
+ // Turret back position
+ for (TurretState turret_pos :
+ {TurretState::kSafeFront, TurretState::kSafeBack,
+ TurretState::kSafeFrontWrapped, TurretState::kSafeBackWrapped,
+ TurretState::kUnsafeFront, TurretState::kUnsafeBack,
+ TurretState::kUnsafeFrontWrapped,
+ TurretState::kUnsafeBackWrapped}) {
+ // Intake front goal
+ for (IntakeState intake_front_goal :
+ {IntakeState::kSafe, IntakeState::kUnsafe}) {
+ // Intake back goal
+ for (IntakeState intake_back_goal :
+ {IntakeState::kSafe, IntakeState::kUnsafe}) {
+ // Turret goal
+ for (TurretState turret_goal :
+ {TurretState::kSafeFront, TurretState::kSafeBack,
+ TurretState::kSafeFrontWrapped, TurretState::kSafeBackWrapped,
+ TurretState::kUnsafeFront, TurretState::kUnsafeBack,
+ TurretState::kUnsafeFrontWrapped,
+ TurretState::kUnsafeBackWrapped}) {
+ Test(intake_front_pos, intake_back_pos, turret_pos,
+ intake_front_goal, intake_back_goal, turret_goal);
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+// Tests some of the various wraps to make sure they match what we expect.
+TEST(WrapTest, Wrap) {
+ EXPECT_THAT(WrapTurretAngle(0.0), ::testing::Eq(std::make_pair(0.0, 0)));
+
+ EXPECT_THAT(WrapTurretAngle(M_PI * 2.0 - 0.1),
+ ::testing::Pair(::testing::DoubleNear(M_PI * 2.0 - 0.1, 1e-6),
+ ::testing::Eq(0)));
+
+ EXPECT_THAT(
+ WrapTurretAngle(M_PI * 2.0 + 0.1),
+ ::testing::Pair(::testing::DoubleNear(0.1, 1e-6), ::testing::Eq(1)));
+
+ EXPECT_THAT(WrapTurretAngle(M_PI * 4.0 - 0.1),
+ ::testing::Pair(::testing::DoubleNear(M_PI * 2.0 - 0.1, 1e-6),
+ ::testing::Eq(1)));
+
+ EXPECT_THAT(
+ WrapTurretAngle(M_PI * 4.0 + 0.1),
+ ::testing::Pair(::testing::DoubleNear(0.1, 1e-6), ::testing::Eq(2)));
+
+ EXPECT_THAT(
+ WrapTurretAngle(-M_PI * 2.0 + 0.1),
+ ::testing::Pair(::testing::DoubleNear(0.1, 1e-6), ::testing::Eq(-1)));
+ EXPECT_THAT(WrapTurretAngle(-0.1),
+ ::testing::Pair(::testing::DoubleNear(M_PI * 2.0 - 0.1, 1e-6),
+ ::testing::Eq(-1)));
+
+ EXPECT_THAT(
+ WrapTurretAngle(-M_PI * 4.0 + 0.1),
+ ::testing::Pair(::testing::DoubleNear(0.1, 1e-6), ::testing::Eq(-2)));
+ EXPECT_THAT(WrapTurretAngle(-M_PI * 2.0 - 0.1),
+ ::testing::Pair(::testing::DoubleNear(M_PI * 2.0 - 0.1, 1e-6),
+ ::testing::Eq(-2)));
+
+ EXPECT_THAT(
+ WrapTurretAngle(-M_PI * 6.0 + 0.1),
+ ::testing::Pair(::testing::DoubleNear(0.1, 1e-6), ::testing::Eq(-3)));
+ EXPECT_THAT(WrapTurretAngle(-M_PI * 4.0 - 0.1),
+ ::testing::Pair(::testing::DoubleNear(M_PI * 2.0 - 0.1, 1e-6),
+ ::testing::Eq(-3)));
+}
+
+// Tests that wrap -> unwrap is a nop.
+TEST(WrapTest, UnWrap) {
+ int wraps = -10000;
+ double wrapped = 0.0;
+ for (double i = -50; i < 50; i += 0.01) {
+ std::pair<double, int> r = WrapTurretAngle(i);
+
+ // Do a dummy check. The wraps should always increase since the angle is
+ // increasing, the wrapped angle too, and everything should be within a
+ // decent range.
+ EXPECT_GE(r.first, 0.0);
+ EXPECT_LT(r.first, 2.0 * M_PI);
+ if (wraps != r.second) {
+ EXPECT_GT(r.second, wraps);
+ wraps = r.second;
+ } else {
+ EXPECT_GT(r.first, wrapped);
+ }
+ wrapped = r.first;
+
+ // And unwrapping should get us back to the start.
+ EXPECT_THAT(UnwrapTurretAngle(r.first, r.second),
+ ::testing::DoubleNear(i, 1e-9));
+ }
+}
+
+} // namespace y2022::control_loops::superstructure::testing
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();
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 68260fd..dfd4265 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -6,6 +6,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/catapult/catapult.h"
+#include "y2022/control_loops/superstructure/collision_avoidance.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
@@ -29,7 +30,7 @@
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
explicit Superstructure(::aos::EventLoop *event_loop,
- std::shared_ptr<const constants::Values> values,
+ std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
inline const PotAndAbsoluteEncoderSubsystem &intake_front() const {
@@ -58,6 +59,8 @@
PotAndAbsoluteEncoderSubsystem intake_back_;
PotAndAbsoluteEncoderSubsystem turret_;
+ CollisionAvoidance collision_avoidance_;
+
aos::Fetcher<frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 6147851..06dee7b 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -282,6 +282,10 @@
class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
public:
+ static constexpr double kSafeTurretAngle =
+ CollisionAvoidance::kMaxCollisionZoneBackTurret +
+ CollisionAvoidance::kEpsTurret;
+
SuperstructureTest()
: ::frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig("y2022/aos_config.json"),
@@ -472,8 +476,7 @@
constants::Values::kIntakeRange().middle());
superstructure_plant_.intake_back()->InitializePosition(
constants::Values::kIntakeRange().middle());
- superstructure_plant_.turret()->InitializePosition(
- constants::Values::kTurretRange().middle());
+ superstructure_plant_.turret()->InitializePosition(kSafeTurretAngle);
superstructure_plant_.catapult()->InitializePosition(
constants::Values::kCatapultRange().middle());
superstructure_plant_.climber()->InitializePosition(
@@ -494,7 +497,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kTurretRange().middle());
+ *builder.fbb(), kSafeTurretAngle);
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
@@ -575,6 +578,8 @@
// behaviour.
TEST_F(SuperstructureTest, SaturationTest) {
SetEnabled(true);
+ superstructure_plant_.turret()->InitializePosition(kSafeTurretAngle);
+
// Zero it before we move.
WaitUntilZeroed();
{
@@ -582,19 +587,21 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().upper);
+ *builder.fbb(), constants::Values::kIntakeRange().lower);
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().upper);
+ *builder.fbb(), constants::Values::kIntakeRange().lower);
+ // Keep the turret away from the intakes because they start in the collision
+ // area
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kTurretRange().upper);
+ *builder.fbb(), kSafeTurretAngle);
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kClimberRange().upper);
+ *builder.fbb(), constants::Values::kClimberRange().lower);
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -607,7 +614,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- RunFor(chrono::seconds(10));
+ RunFor(chrono::seconds(20));
VerifyNearGoal();
// Try a low acceleration move with a high max velocity and verify the
@@ -617,22 +624,22 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_front = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().lower,
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset_back = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kIntakeRange().lower,
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kTurretRange().lower,
+ *builder.fbb(), constants::Values::kTurretRange().upper,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kClimberRange().lower,
+ *builder.fbb(), constants::Values::kClimberRange().upper,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -723,6 +730,7 @@
constants::Values::kIntakeRange().middle());
superstructure_plant_.intake_back()->InitializePosition(
constants::Values::kIntakeRange().middle());
+ superstructure_plant_.turret()->InitializePosition(kSafeTurretAngle);
WaitUntilZeroed();
@@ -742,10 +750,17 @@
*builder.fbb(), constants::Values::kIntakeRange().upper,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+ // Keep the turret away from the intakes to not trigger collision avoidance
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), kSafeTurretAngle,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_front(intake_offset_front);
goal_builder.add_intake_back(intake_offset_back);
+ goal_builder.add_turret(turret_offset);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
// TODO(Milo): Make this a sane time