Add turret to intake collision avoidance
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Idca062b4ff7ddca289581baf032736dadbf71e8c
diff --git a/y2024/control_loops/superstructure/BUILD b/y2024/control_loops/superstructure/BUILD
index 0493c9b..6150caa 100644
--- a/y2024/control_loops/superstructure/BUILD
+++ b/y2024/control_loops/superstructure/BUILD
@@ -81,6 +81,7 @@
data = [
],
deps = [
+ ":collision_avoidance_lib",
":shooter",
":superstructure_goal_fbs",
":superstructure_output_fbs",
@@ -113,6 +114,36 @@
],
)
+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",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "@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",
+ ],
+)
+
cc_test(
name = "superstructure_lib_test",
srcs = [
@@ -151,6 +182,7 @@
],
deps = [
":aiming",
+ ":collision_avoidance_lib",
":superstructure_can_position_fbs",
":superstructure_goal_fbs",
":superstructure_position_fbs",
diff --git a/y2024/control_loops/superstructure/collision_avoidance.cc b/y2024/control_loops/superstructure/collision_avoidance.cc
new file mode 100644
index 0000000..0594af0
--- /dev/null
+++ b/y2024/control_loops/superstructure/collision_avoidance.cc
@@ -0,0 +1,110 @@
+#include "y2024/control_loops/superstructure/collision_avoidance.h"
+
+#include <cmath>
+
+#include "absl/functional/bind_front.h"
+#include "glog/logging.h"
+
+namespace y2024::control_loops::superstructure {
+
+CollisionAvoidance::CollisionAvoidance() {
+ clear_min_intake_pivot_goal();
+ clear_max_intake_pivot_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_pivot_position, status.turret_position,
+ kMinCollisionZoneTurret, kMaxCollisionZoneTurret)) {
+ return true;
+ }
+
+ return false;
+}
+
+bool AngleInRange(double theta, double theta_min, double theta_max) {
+ return (
+ (theta >= theta_min && theta <= theta_max) ||
+ (theta_min > theta_max && (theta >= theta_min || theta <= theta_max)));
+}
+
+bool CollisionAvoidance::TurretCollided(double intake_position,
+ double turret_position,
+ double min_turret_collision_position,
+ double max_turret_collision_position) {
+ // Checks if turret is in the collision area.
+ if (AngleInRange(turret_position, min_turret_collision_position,
+ max_turret_collision_position)) {
+ // Returns 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 double &turret_goal_position) {
+ // Start with our constraints being wide open.
+ clear_max_turret_goal();
+ clear_min_turret_goal();
+ clear_max_intake_pivot_goal();
+ clear_min_intake_pivot_goal();
+
+ const double intake_pivot_position = status.intake_pivot_position;
+ const double turret_position = status.turret_position;
+
+ const double turret_goal = turret_goal_position;
+
+ // Calculating the avoidance with the intake
+
+ CalculateAvoidance(true, intake_pivot_position, turret_position, turret_goal,
+ kMinCollisionZoneTurret, kMaxCollisionZoneTurret);
+}
+
+void CollisionAvoidance::CalculateAvoidance(bool intake_pivot,
+ double intake_position,
+ double turret_position,
+ double turret_goal,
+ double min_turret_collision_goal,
+ double max_turret_collision_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_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)));
+
+ if (turret_pos_unsafe || turret_moving_past_intake) {
+ // If the turret is unsafe, limit the intake
+ if (intake_pivot) {
+ 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_position <
+ (min_turret_collision_goal + max_turret_collision_goal / 2)) {
+ update_max_turret_goal(min_turret_collision_goal - kEpsTurret);
+ } else {
+ update_min_turret_goal(max_turret_collision_goal + kEpsTurret);
+ }
+ }
+ }
+}
+
+} // namespace y2024::control_loops::superstructure
diff --git a/y2024/control_loops/superstructure/collision_avoidance.h b/y2024/control_loops/superstructure/collision_avoidance.h
new file mode 100644
index 0000000..8fd14d7
--- /dev/null
+++ b/y2024/control_loops/superstructure/collision_avoidance.h
@@ -0,0 +1,109 @@
+#ifndef Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDENCE_H_
+#define Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDENCE_H_
+
+#include <cmath>
+
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+
+namespace y2024::control_loops::superstructure {
+
+// Checks if theta is between theta_min and theta_max. Expects all angles to be
+// wrapped from 0 to 2pi
+bool AngleInRange(double theta, double theta_min, double theta_max);
+
+// 1. Prevent the turret from moving if the intake is up
+// and prevent the back of the turret from colliding with the intake when it's
+// 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_pivot_position;
+ double turret_position;
+
+ bool operator==(const Status &s) const {
+ return (intake_pivot_position == s.intake_pivot_position &&
+ turret_position == s.turret_position);
+ }
+ bool operator!=(const Status &s) const { return !(*this == s); }
+ };
+
+ // Reference angles between which the turret will be careful
+ static constexpr double kCollisionZoneTurret = M_PI * 7.0 / 18.0;
+
+ // For the turret, 0 rad is pointing straight forwards
+ static constexpr double kMinCollisionZoneTurret = 0.07;
+ static constexpr double kMaxCollisionZoneTurret = 2.1;
+
+ // Maximum position of the intake to avoid collisions
+ static constexpr double kCollisionZoneIntake = 1.6;
+
+ // Tolerances for the subsystems
+ static constexpr double kEpsTurret = 0.05;
+ 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 CollisionAvoidance::Status &status,
+ const double &turret_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);
+
+ // 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_pivot_goal() const { return min_intake_pivot_goal_; }
+ double max_intake_pivot_goal() const { return max_intake_pivot_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_pivot_goal(double max_intake_pivot_goal) {
+ max_intake_pivot_goal_ =
+ ::std::min(max_intake_pivot_goal, max_intake_pivot_goal_);
+ }
+ void update_min_intake_pivot_goal(double min_intake_pivot_goal) {
+ min_intake_pivot_goal_ =
+ ::std::max(min_intake_pivot_goal, min_intake_pivot_goal_);
+ }
+
+ private:
+ void clear_min_intake_pivot_goal() {
+ min_intake_pivot_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_intake_pivot_goal() {
+ max_intake_pivot_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_pivot_goal_;
+ double max_intake_pivot_goal_;
+ double min_turret_goal_;
+ double max_turret_goal_;
+};
+
+} // namespace y2024::control_loops::superstructure
+
+#endif
diff --git a/y2024/control_loops/superstructure/collision_avoidance_test.cc b/y2024/control_loops/superstructure/collision_avoidance_test.cc
new file mode 100644
index 0000000..6425120
--- /dev/null
+++ b/y2024/control_loops/superstructure/collision_avoidance_test.cc
@@ -0,0 +1,187 @@
+#include "y2024/control_loops/superstructure/collision_avoidance.h"
+
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "aos/commonmath.h"
+#include "aos/flatbuffers.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2024::control_loops::superstructure::testing {
+
+using aos::FlatbufferDetachedBuffer;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+// Enums for the different classes of intake and turret states
+enum class IntakeState { kSafe, kUnsafe };
+enum class TurretState {
+ kSafe,
+ kUnsafe,
+};
+enum class CatapultState { kIdle, kShooting };
+
+class CollisionAvoidanceTest : public ::testing::Test {
+ public:
+ CollisionAvoidanceTest()
+ : intake_goal_(0),
+ turret_goal_(0),
+ status_({0.0, 0.0}),
+ prev_status_({0.0, 0.0}) {}
+
+ void Simulate() {
+ double safe_intake_goal = 0;
+ double safe_turret_goal = 0;
+ bool was_collided = avoidance_.IsCollided(status_);
+
+ bool moving = true;
+ while (moving) {
+ // Compute the safe goal
+ avoidance_.UpdateGoal(status_, turret_goal_);
+
+ if (!was_collided) {
+ // The system should never be collided if it didn't start off collided
+ EXPECT_FALSE(avoidance_.IsCollided(status_));
+ } else {
+ was_collided = avoidance_.IsCollided(status_);
+ }
+
+ safe_intake_goal =
+ ::aos::Clip(intake_goal_, avoidance_.min_intake_pivot_goal(),
+ avoidance_.max_intake_pivot_goal());
+
+ safe_turret_goal = ::aos::Clip(turret_goal_, avoidance_.min_turret_goal(),
+ avoidance_.max_turret_goal());
+
+ // Move each subsystem towards their goals a bit
+ status_.intake_pivot_position =
+ LimitedMove(status_.intake_pivot_position, safe_intake_goal);
+ status_.turret_position =
+ LimitedMove(status_.turret_position, safe_turret_goal);
+
+ // If it stopped moving, we're done
+ if (!IsMoving()) {
+ moving = false;
+ } else {
+ prev_status_ = status_;
+ }
+ }
+
+ EXPECT_FALSE(avoidance_.IsCollided(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;
+
+ switch (turret_state) {
+ case TurretState::kSafe:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneTurret +
+ CollisionAvoidance::kEpsTurret;
+ break;
+ case TurretState::kUnsafe:
+ turret_angle = CollisionAvoidance::kMaxCollisionZoneTurret -
+ CollisionAvoidance::kEpsTurret;
+ break;
+ }
+
+ return turret_angle;
+ }
+
+ void Test(IntakeState intake_front_pos_state, TurretState turret_pos_state,
+ IntakeState intake_front_goal_state,
+ TurretState turret_goal_state) {
+ status_ = {ComputeIntakeAngle(intake_front_pos_state),
+ ComputeTurretAngle(turret_pos_state)};
+
+ intake_goal_ = ComputeIntakeAngle(intake_front_goal_state);
+
+ turret_goal_ = ComputeTurretAngle(turret_goal_state);
+
+ Simulate();
+ }
+
+ // Provide goals and status messages
+ double intake_goal_;
+ double turret_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_goal_, turret_goal_,
+ CollisionAvoidance::kMinCollisionZoneTurret,
+ CollisionAvoidance::kMaxCollisionZoneTurret)) {
+ EXPECT_LE(status_.intake_pivot_position,
+ CollisionAvoidance::kCollisionZoneIntake);
+ } else {
+ EXPECT_NEAR(intake_goal_, status_.intake_pivot_position, kIterationMove);
+ }
+ }
+
+ double LimitedMove(double position, double goal) {
+ if (position + kIterationMove < goal) {
+ return position + kIterationMove;
+ } else if (position - kIterationMove > goal) {
+ return position - kIterationMove;
+ } else {
+ return 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}) {
+ // Turret back position
+ for (TurretState turret_pos : {TurretState::kSafe, TurretState::kUnsafe}) {
+ // Intake front goal
+ for (IntakeState intake_front_goal :
+ {IntakeState::kSafe, IntakeState::kUnsafe}) {
+ // Turret goal
+ for (TurretState turret_goal :
+ {TurretState::kSafe, TurretState::kUnsafe}) {
+ Test(intake_front_pos, turret_pos, intake_front_goal, turret_goal);
+ }
+ }
+ }
+ }
+}
+
+} // namespace y2024::control_loops::superstructure::testing
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 0666772..a0dd234 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -4,6 +4,7 @@
#include "aos/flatbuffers/base.h"
#include "frc971/control_loops/aiming/aiming.h"
#include "y2024/control_loops/superstructure/catapult/catapult_plant.h"
+#include "y2024/control_loops/superstructure/collision_avoidance.h"
namespace y2024::control_loops::superstructure {
@@ -43,6 +44,8 @@
double *catapult_output, double *altitude_output, double *turret_output,
double *retention_roller_output, double /*battery_voltage*/,
aos::monotonic_clock::time_point current_timestamp,
+ CollisionAvoidance *collision_avoidance, const double intake_pivot_position,
+ double *max_intake_pivot_position, double *min_intake_pivot_position,
flatbuffers::FlatBufferBuilder *fbb) {
superstructure_can_position_fetcher_.Fetch();
drivetrain_status_fetcher_.Fetch();
@@ -146,9 +149,32 @@
altitude_.estimated_position() >
robot_constants_->common()->min_altitude_shooting_angle());
+ const bool disabled = turret_.Correct(turret_goal, position->turret(),
+ turret_output == nullptr);
+
+ collision_avoidance->UpdateGoal(
+ {.intake_pivot_position = intake_pivot_position,
+ .turret_position = turret_.estimated_position()},
+ turret_goal->unsafe_goal());
+
+ turret_.set_min_position(collision_avoidance->min_turret_goal());
+ turret_.set_max_position(collision_avoidance->max_turret_goal());
+
+ *max_intake_pivot_position = collision_avoidance->max_intake_pivot_goal();
+ *min_intake_pivot_position = collision_avoidance->min_intake_pivot_goal();
+
+ // Calculate the loops for a cycle.
+ const double voltage = turret_.UpdateController(disabled);
+
+ turret_.UpdateObserver(voltage);
+
+ // Write out all the voltages.
+ if (turret_output) {
+ *turret_output = voltage;
+ }
+
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
- turret_status_offset =
- turret_.Iterate(turret_goal, position->turret(), turret_output, fbb);
+ turret_status_offset = turret_.MakeStatus(fbb);
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
altitude_status_offset = altitude_.Iterate(
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index b381f3e..048e4b2 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -9,6 +9,7 @@
#include "y2024/constants.h"
#include "y2024/constants/constants_generated.h"
#include "y2024/control_loops/superstructure/aiming.h"
+#include "y2024/control_loops/superstructure/collision_avoidance.h"
#include "y2024/control_loops/superstructure/superstructure_can_position_generated.h"
#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
@@ -67,7 +68,10 @@
double *catapult_output, double *altitude_output, double *turret_output,
double *retention_roller_output, double battery_voltage,
aos::monotonic_clock::time_point current_timestamp,
- flatbuffers::FlatBufferBuilder *fbb);
+ /* Hacky way to use collision avoidance in this class */
+ CollisionAvoidance *collision_avoidance,
+ const double intake_pivot_position, double *max_turret_intake_position,
+ double *min_intake_pivot_position, flatbuffers::FlatBufferBuilder *fbb);
private:
CatapultState state_ = CatapultState::RETRACTING;
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 5ac705b..ebcbf1b 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -140,22 +140,9 @@
drivetrain_status_fetcher_.Fetch();
- aos::FlatbufferFixedAllocatorArray<
- frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
- intake_pivot_goal_buffer;
-
- intake_pivot_goal_buffer.Finish(
- frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *intake_pivot_goal_buffer.fbb(), intake_pivot_position));
-
- const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
- *intake_pivot_goal = &intake_pivot_goal_buffer.message();
-
- const flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
- intake_pivot_status_offset = intake_pivot_.Iterate(
- intake_pivot_goal, position->intake_pivot(),
- output != nullptr ? &output_struct.intake_pivot_voltage : nullptr,
- status->fbb());
+ const bool collided = collision_avoidance_.IsCollided(
+ {.intake_pivot_position = intake_pivot_.estimated_position(),
+ .turret_position = shooter_.turret().estimated_position()});
aos::FlatbufferFixedAllocatorArray<
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
@@ -174,6 +161,28 @@
output != nullptr ? &output_struct.climber_voltage : nullptr,
status->fbb());
+ double max_intake_pivot_position = 0;
+ double min_intake_pivot_position = 0;
+
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
+ intake_pivot_goal_buffer;
+
+ intake_pivot_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *intake_pivot_goal_buffer.fbb(), intake_pivot_position));
+
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *intake_pivot_goal = &intake_pivot_goal_buffer.message();
+
+ double *intake_output =
+ (output != nullptr ? &output_struct.intake_pivot_voltage : nullptr);
+
+ const bool disabled = intake_pivot_.Correct(
+ intake_pivot_goal, position->intake_pivot(), intake_output == nullptr);
+
+ // TODO(max): Change how we handle the collision with the turret and intake to
+ // be clearer
const flatbuffers::Offset<ShooterStatus> shooter_status_offset =
shooter_.Iterate(
position,
@@ -182,7 +191,25 @@
output != nullptr ? &output_struct.altitude_voltage : nullptr,
output != nullptr ? &output_struct.turret_voltage : nullptr,
output != nullptr ? &output_struct.retention_roller_voltage : nullptr,
- robot_state().voltage_battery(), timestamp, status->fbb());
+ robot_state().voltage_battery(), timestamp, &collision_avoidance_,
+ intake_pivot_.estimated_position(), &max_intake_pivot_position,
+ &min_intake_pivot_position, status->fbb());
+
+ intake_pivot_.set_min_position(min_intake_pivot_position);
+ intake_pivot_.set_max_position(max_intake_pivot_position);
+
+ // Calculate the loops for a cycle.
+ const double voltage = intake_pivot_.UpdateController(disabled);
+
+ intake_pivot_.UpdateObserver(voltage);
+
+ // Write out all the voltages.
+ if (intake_output) {
+ *intake_output = voltage;
+ }
+
+ const flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
+ intake_pivot_status_offset = intake_pivot_.MakeStatus(status->fbb());
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
@@ -202,6 +229,7 @@
status_builder.add_transfer_roller(transfer_roller_state);
status_builder.add_climber(climber_status_offset);
status_builder.add_shooter(shooter_status_offset);
+ status_builder.add_collided(collided);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 6c43863..650092b 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -61,6 +61,8 @@
drivetrain_status_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ CollisionAvoidance collision_avoidance_;
+
aos::Alliance alliance_ = aos::Alliance::kInvalid;
TransferRollerGoal transfer_goal_;
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index 39f108a..a57422b 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -12,8 +12,8 @@
// Represents goal for pivot on intake
enum IntakePivotGoal : ubyte {
- EXTENDED = 0,
- RETRACTED = 1,
+ RETRACTED = 0,
+ EXTENDED = 1,
}
// Represents goal of transfer rollers
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index fbabcfb..eeeb731 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -383,9 +383,10 @@
ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr)
<< ": No output";
- double set_point = simulated_robot_constants_->common()
- ->intake_pivot_set_points()
- ->retracted();
+ EXPECT_FALSE(superstructure_status_fetcher_->collided());
+
+ double set_point = superstructure_status_fetcher_->intake_pivot()
+ ->unprofiled_goal_position();
if (superstructure_goal_fetcher_->intake_pivot_goal() ==
IntakePivotGoal::EXTENDED) {
@@ -427,10 +428,6 @@
}
}
- EXPECT_NEAR(set_point,
- superstructure_status_fetcher_->intake_pivot()->position(),
- 0.001);
-
if (superstructure_status_fetcher_->intake_roller() ==
IntakeRollerStatus::NONE) {
EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(), 0.0);
@@ -591,16 +588,14 @@
shooter_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
goal_builder.add_climber_goal(ClimberGoal::RETRACT);
goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(10));
- EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
-
VerifyNearGoal();
}
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index 7b7912e..872fb21 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -91,6 +91,9 @@
// State of the extender rollers
extend_roller:ExtendRollerStatus (id: 8);
+
+ // The status of if the turret and intake is colliding
+ collided: bool (id: 9);
}
root_type Status;