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
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
diff --git a/y2024/control_loops/superstructure/collision_avoidance_test.cc b/y2024/control_loops/superstructure/collision_avoidance_test.cc
index 6425120..c3a8e3a 100644
--- a/y2024/control_loops/superstructure/collision_avoidance_test.cc
+++ b/y2024/control_loops/superstructure/collision_avoidance_test.cc
@@ -20,29 +20,45 @@
kSafe,
kUnsafe,
};
-enum class CatapultState { kIdle, kShooting };
+enum class ExtendState {
+ kSafe,
+ kUnsafe,
+};
class CollisionAvoidanceTest : public ::testing::Test {
public:
CollisionAvoidanceTest()
: intake_goal_(0),
turret_goal_(0),
- status_({0.0, 0.0}),
- prev_status_({0.0, 0.0}) {}
+ extend_goal_(0),
+ status_({0.0, 0.0, 0.0}),
+ prev_status_({0.0, 0.0, 0.0}) {}
void Simulate() {
double safe_intake_goal = 0;
double safe_turret_goal = 0;
+ double safe_extend_goal = 0;
bool was_collided = avoidance_.IsCollided(status_);
+ VLOG(1) << "Simulation of intake " << status_.intake_pivot_position
+ << ", intake_goal " << intake_goal_ << ", turret "
+ << status_.turret_position << ", turret_goal " << turret_goal_
+ << ", extend " << status_.extend_position << ", extend goal "
+ << extend_goal_;
+
bool moving = true;
while (moving) {
// Compute the safe goal
- avoidance_.UpdateGoal(status_, turret_goal_);
+ avoidance_.UpdateGoal(status_, turret_goal_, extend_goal_);
if (!was_collided) {
// The system should never be collided if it didn't start off collided
- EXPECT_FALSE(avoidance_.IsCollided(status_));
+ EXPECT_FALSE(avoidance_.IsCollided(status_))
+ << ": Now collided at intake " << status_.intake_pivot_position
+ << ", intake_goal " << intake_goal_ << ", turret "
+ << status_.turret_position << ", turret_goal " << turret_goal_
+ << ", extend " << status_.extend_position << ", extend goal "
+ << extend_goal_;
} else {
was_collided = avoidance_.IsCollided(status_);
}
@@ -54,11 +70,16 @@
safe_turret_goal = ::aos::Clip(turret_goal_, avoidance_.min_turret_goal(),
avoidance_.max_turret_goal());
+ safe_extend_goal = ::aos::Clip(extend_goal_, avoidance_.min_extend_goal(),
+ avoidance_.max_extend_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);
+ status_.extend_position =
+ LimitedMove(status_.extend_position, safe_extend_goal);
// If it stopped moving, we're done
if (!IsMoving()) {
@@ -109,22 +130,43 @@
return turret_angle;
}
+ double ComputeExtendPosition(ExtendState extend_state) {
+ double extend_position = 0.0;
+
+ switch (extend_state) {
+ case ExtendState::kSafe:
+ extend_position = CollisionAvoidance::kMinCollisionZoneExtend -
+ CollisionAvoidance::kEpsExtend;
+ break;
+ case ExtendState::kUnsafe:
+ extend_position = CollisionAvoidance::kMinCollisionZoneExtend +
+ CollisionAvoidance::kEpsTurret;
+ break;
+ }
+
+ return extend_position;
+ }
+
void Test(IntakeState intake_front_pos_state, TurretState turret_pos_state,
- IntakeState intake_front_goal_state,
- TurretState turret_goal_state) {
+ ExtendState extend_pos_state, IntakeState intake_front_goal_state,
+ TurretState turret_goal_state, ExtendState extend_goal_state) {
status_ = {ComputeIntakeAngle(intake_front_pos_state),
- ComputeTurretAngle(turret_pos_state)};
+ ComputeTurretAngle(turret_pos_state),
+ ComputeExtendPosition(extend_pos_state)};
intake_goal_ = ComputeIntakeAngle(intake_front_goal_state);
turret_goal_ = ComputeTurretAngle(turret_goal_state);
+ extend_goal_ = ComputeExtendPosition(extend_goal_state);
+
Simulate();
}
// Provide goals and status messages
double intake_goal_;
double turret_goal_;
+ double extend_goal_;
CollisionAvoidance::Status status_;
private:
@@ -133,19 +175,24 @@
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);
+ EXPECT_NEAR(extend_goal_, status_.extend_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);
+ if (avoidance_.ExtendCollided(intake_goal_, turret_goal_, extend_goal_)) {
+ EXPECT_EQ(status_.turret_position,
+ CollisionAvoidance::kSafeTurretExtendedPosition);
+ // 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_, status_.turret_position,
+ extend_goal_)) {
+ EXPECT_LE(status_.intake_pivot_position,
+ CollisionAvoidance::kCollisionZoneIntake);
+ } else {
+ EXPECT_NEAR(intake_goal_, status_.intake_pivot_position,
+ kIterationMove);
+ }
} else {
- EXPECT_NEAR(intake_goal_, status_.intake_pivot_position, kIterationMove);
+ EXPECT_NEAR(turret_goal_, status_.turret_position, kIterationMove);
}
}
@@ -161,7 +208,7 @@
CollisionAvoidance avoidance_;
CollisionAvoidance::Status prev_status_;
-};
+}; // namespace y2024::control_loops::superstructure::testing
// Just to be safe, brute force ALL the possible position-goal combinations
// and make sure we never collide and the correct goals are reached
@@ -171,13 +218,20 @@
{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);
+ for (ExtendState extend_pos :
+ {ExtendState::kSafe, ExtendState::kUnsafe}) {
+ // Intake front goal
+ for (IntakeState intake_front_goal :
+ {IntakeState::kSafe, IntakeState::kUnsafe}) {
+ // Turret goal
+ for (TurretState turret_goal :
+ {TurretState::kSafe, TurretState::kUnsafe}) {
+ for (ExtendState extend_goal :
+ {ExtendState::kSafe, ExtendState::kUnsafe}) {
+ Test(intake_front_pos, turret_pos, extend_pos, intake_front_goal,
+ turret_goal, extend_goal);
+ }
+ }
}
}
}
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 07b7ce1..cc00454 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -40,9 +40,11 @@
bool fire, double *catapult_output, double *altitude_output,
double *turret_output, double *retention_roller_output,
double *retention_roller_stator_current_limit, double /*battery_voltage*/,
- CollisionAvoidance *collision_avoidance, const double intake_pivot_position,
+ CollisionAvoidance *collision_avoidance, const double extend_position,
+ const double extend_goal, double *max_extend_position,
+ double *min_extend_position, const double intake_pivot_position,
double *max_intake_pivot_position, double *min_intake_pivot_position,
- bool standby, flatbuffers::FlatBufferBuilder *fbb,
+ flatbuffers::FlatBufferBuilder *fbb,
aos::monotonic_clock::time_point monotonic_now) {
drivetrain_status_fetcher_.Fetch();
@@ -94,18 +96,8 @@
bool aiming = false;
- if (standby) {
- // Note is going to AMP or TRAP, move turret to extend
- // collision avoidance position.
- PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
- turret_goal_builder.get(),
- robot_constants_->common()->turret_avoid_extend_collision_position());
-
- PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
- altitude_goal_builder.get(),
- robot_constants_->common()->altitude_loading_position());
- } else if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
- (!piece_loaded && state_ == CatapultState::READY)) {
+ if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
+ (!piece_loaded && state_ == CatapultState::READY)) {
// We don't have the note so we should be ready to intake it.
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
turret_goal_builder.get(),
@@ -138,7 +130,7 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*turret_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
- !standby && shooter_goal->has_turret_position())
+ shooter_goal->has_turret_position())
? shooter_goal->turret_position()
: &turret_goal_builder->AsFlatbuffer();
@@ -161,8 +153,9 @@
collision_avoidance->UpdateGoal(
{.intake_pivot_position = intake_pivot_position,
- .turret_position = turret_.estimated_position()},
- turret_goal->unsafe_goal());
+ .turret_position = turret_.estimated_position(),
+ .extend_position = extend_position},
+ turret_goal->unsafe_goal(), extend_goal);
turret_.set_min_position(collision_avoidance->min_turret_goal());
turret_.set_max_position(collision_avoidance->max_turret_goal());
@@ -170,6 +163,9 @@
*max_intake_pivot_position = collision_avoidance->max_intake_pivot_goal();
*min_intake_pivot_position = collision_avoidance->min_intake_pivot_goal();
+ *max_extend_position = collision_avoidance->max_extend_goal();
+ *min_extend_position = collision_avoidance->min_extend_goal();
+
// Calculate the loops for a cycle.
const double voltage = turret_.UpdateController(disabled);
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 4ceffd4..2571cab 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -102,10 +102,10 @@
double *retention_roller_output,
double *retention_roller_stator_current_limit, double battery_voltage,
/* 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,
- /* If true, go to extend collision avoidance position */ bool standby,
+ CollisionAvoidance *collision_avoidance, const double extend_position,
+ const double extend_goal, double *max_extend_position,
+ double *min_extend_position, const double intake_pivot_position,
+ double *max_turret_intake_position, double *min_intake_pivot_position,
flatbuffers::FlatBufferBuilder *fbb,
aos::monotonic_clock::time_point monotonic_now);
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 1c37017..ad91614 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -128,7 +128,7 @@
}
ExtendRollerStatus extend_roller_status = ExtendRollerStatus::IDLE;
- ExtendStatus extend_goal = ExtendStatus::RETRACTED;
+ ExtendStatus extend_goal_location = ExtendStatus::RETRACTED;
// True if the extend is moving towards a goal
bool extend_moving = false;
@@ -149,12 +149,6 @@
robot_constants_->common()->turret_loading_position(),
kTurretLoadingThreshold);
- // Check if turret is at the right position for the extend to extend.
- const bool turret_ready_for_extend_move = PositionNear(
- shooter_.turret().estimated_position(),
- robot_constants_->common()->turret_avoid_extend_collision_position(),
- kTurretLoadingThreshold);
-
// Check if the altitude is at the position to accept the note from
// extend
const bool altitude_ready_for_load =
@@ -166,10 +160,6 @@
const bool extend_ready_for_catapult_transfer = PositionNear(
extend_.position(), extend_set_points->catapult(), kExtendThreshold);
- // If true, the turret should be moved to the position to avoid collision with
- // the extend.
- bool move_turret_to_standby = false;
-
// Only update the reuested note goal to the first goal that is requested by
// the manipulator
if (unsafe_goal != nullptr && unsafe_goal->note_goal() != NoteGoal::NONE &&
@@ -210,7 +200,7 @@
state_ = SuperstructureState::INTAKING;
}
- extend_goal = ExtendStatus::RETRACTED;
+ extend_goal_location = ExtendStatus::RETRACTED;
catapult_requested_ = false;
break;
case SuperstructureState::INTAKING:
@@ -222,7 +212,7 @@
intake_roller_state = IntakeRollerStatus::INTAKING;
transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_EXTEND;
- extend_goal = ExtendStatus::RETRACTED;
+ extend_goal_location = ExtendStatus::RETRACTED;
if (!catapult_requested_ && unsafe_goal != nullptr &&
unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
@@ -254,35 +244,29 @@
[[fallthrough]];
case NoteGoal::AMP:
transfer_roller_status = TransferRollerStatus::EXTEND_MOVING;
- if (turret_ready_for_extend_move) {
- state_ = SuperstructureState::MOVING;
- } else {
- move_turret_to_standby = true;
- }
+ state_ = SuperstructureState::MOVING;
break;
}
- extend_goal = ExtendStatus::RETRACTED;
+ extend_goal_location = ExtendStatus::RETRACTED;
break;
case SuperstructureState::MOVING:
transfer_roller_status = TransferRollerStatus::EXTEND_MOVING;
switch (requested_note_goal_) {
case NoteGoal::NONE:
- extend_goal = ExtendStatus::RETRACTED;
- move_turret_to_standby = true;
+ extend_goal_location = ExtendStatus::RETRACTED;
if (extend_at_retracted) {
state_ = SuperstructureState::LOADED;
}
break;
case NoteGoal::CATAPULT:
- extend_goal = ExtendStatus::CATAPULT;
+ extend_goal_location = ExtendStatus::CATAPULT;
if (extend_ready_for_catapult_transfer && turret_ready_for_load &&
altitude_ready_for_load) {
state_ = SuperstructureState::LOADING_CATAPULT;
}
break;
case NoteGoal::TRAP:
- extend_goal = ExtendStatus::TRAP;
- move_turret_to_standby = true;
+ extend_goal_location = ExtendStatus::TRAP;
// Check if the extend is at the TRAP position and if it is
// switch to READY state
if (PositionNear(extend_.position(), extend_set_points->trap(),
@@ -291,8 +275,7 @@
}
break;
case NoteGoal::AMP:
- extend_goal = ExtendStatus::AMP;
- move_turret_to_standby = true;
+ extend_goal_location = ExtendStatus::AMP;
// Check if the extend is at the AMP position and if it is
// switch to READY state
if (PositionNear(extend_.position(), extend_set_points->amp(),
@@ -306,7 +289,7 @@
break;
case SuperstructureState::LOADING_CATAPULT:
extend_moving = false;
- extend_goal = ExtendStatus::CATAPULT;
+ extend_goal_location = ExtendStatus::CATAPULT;
extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_CATAPULT;
// Switch to READY state when the catapult beambreak is triggered
@@ -324,20 +307,18 @@
switch (requested_note_goal_) {
case NoteGoal::NONE:
- extend_goal = ExtendStatus::RETRACTED;
+ extend_goal_location = ExtendStatus::RETRACTED;
extend_moving = true;
state_ = SuperstructureState::MOVING;
break;
case NoteGoal::CATAPULT:
- extend_goal = ExtendStatus::CATAPULT;
+ extend_goal_location = ExtendStatus::CATAPULT;
break;
case NoteGoal::TRAP:
- extend_goal = ExtendStatus::TRAP;
- move_turret_to_standby = true;
+ extend_goal_location = ExtendStatus::TRAP;
break;
case NoteGoal::AMP:
- extend_goal = ExtendStatus::AMP;
- move_turret_to_standby = true;
+ extend_goal_location = ExtendStatus::AMP;
break;
}
break;
@@ -347,7 +328,7 @@
break;
case NoteGoal::CATAPULT:
- extend_goal = ExtendStatus::CATAPULT;
+ extend_goal_location = ExtendStatus::CATAPULT;
// Reset the state to IDLE when the game piece is fired from the
// catapult. We consider the game piece to be fired from the catapult
// when the catapultbeambreak is no longer triggered.
@@ -357,7 +338,7 @@
break;
case NoteGoal::TRAP:
extend_roller_status = ExtendRollerStatus::SCORING_IN_TRAP;
- extend_goal = ExtendStatus::TRAP;
+ extend_goal_location = ExtendStatus::TRAP;
if (!position->extend_beambreak() && unsafe_goal != nullptr &&
!unsafe_goal->fire()) {
state_ = SuperstructureState::IDLE;
@@ -365,7 +346,7 @@
break;
case NoteGoal::AMP:
extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
- extend_goal = ExtendStatus::AMP;
+ extend_goal_location = ExtendStatus::AMP;
if (!position->extend_beambreak() && unsafe_goal != nullptr &&
!unsafe_goal->fire()) {
state_ = SuperstructureState::IDLE;
@@ -440,32 +421,25 @@
break;
}
- double extend_position = 0.0;
+ double extend_goal_position = 0.0;
if (unsafe_goal != nullptr && unsafe_goal->note_goal() == NoteGoal::TRAP) {
- extend_goal = ExtendStatus::TRAP;
- move_turret_to_standby = true;
- }
-
- // In lieu of having full collision avoidance ready, move the turret out of
- // the way whenever the extend is raised too much.
- if (extend_.position() > 0.05) {
- move_turret_to_standby = true;
+ extend_goal_location = ExtendStatus::TRAP;
}
// Set the extend position based on the state machine output
- switch (extend_goal) {
+ switch (extend_goal_location) {
case ExtendStatus::RETRACTED:
- extend_position = extend_set_points->retracted();
+ extend_goal_position = extend_set_points->retracted();
break;
case ExtendStatus::AMP:
- extend_position = extend_set_points->amp();
+ extend_goal_position = extend_set_points->amp();
break;
case ExtendStatus::TRAP:
- extend_position = extend_set_points->trap();
+ extend_goal_position = extend_set_points->trap();
break;
case ExtendStatus::CATAPULT:
- extend_position = extend_set_points->catapult();
+ extend_goal_position = extend_set_points->catapult();
break;
case ExtendStatus::MOVING:
// Should never happen
@@ -493,7 +467,7 @@
// If the extend is moving, the status is MOVING, otherwise it is the same
// as extend_status
ExtendStatus extend_status =
- (extend_moving ? ExtendStatus::MOVING : extend_goal);
+ (extend_moving ? ExtendStatus::MOVING : extend_goal_location);
if (joystick_state_fetcher_.Fetch() &&
joystick_state_fetcher_->has_alliance()) {
@@ -502,9 +476,11 @@
drivetrain_status_fetcher_.Fetch();
- const bool collided = collision_avoidance_.IsCollided(
- {.intake_pivot_position = intake_pivot_.estimated_position(),
- .turret_position = shooter_.turret().estimated_position()});
+ const bool collided = collision_avoidance_.IsCollided({
+ .intake_pivot_position = intake_pivot_.estimated_position(),
+ .turret_position = shooter_.turret().estimated_position(),
+ .extend_position = extend_.estimated_position(),
+ });
aos::FlatbufferFixedAllocatorArray<
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
@@ -525,6 +501,8 @@
double max_intake_pivot_position = 0;
double min_intake_pivot_position = 0;
+ double max_extend_position = 0;
+ double min_extend_position = 0;
aos::FlatbufferFixedAllocatorArray<
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
@@ -543,6 +521,17 @@
const bool disabled = intake_pivot_.Correct(
intake_pivot_goal, position->intake_pivot(), intake_output == nullptr);
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
+ extend_goal_buffer;
+
+ extend_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *extend_goal_buffer.fbb(), extend_goal_position));
+
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *extend_goal = &extend_goal_buffer.message();
+
// TODO(max): Change how we handle the collision with the turret and
// intake to be clearer
const flatbuffers::Offset<ShooterStatus> shooter_status_offset =
@@ -558,13 +547,17 @@
? &output_struct.retention_roller_stator_current_limit
: nullptr,
robot_state().voltage_battery(), &collision_avoidance_,
+ extend_goal_position, extend_.estimated_position(),
+ &max_extend_position, &min_extend_position,
intake_pivot_.estimated_position(), &max_intake_pivot_position,
- &min_intake_pivot_position, move_turret_to_standby, status->fbb(),
- timestamp);
+ &min_intake_pivot_position, status->fbb(), timestamp);
intake_pivot_.set_min_position(min_intake_pivot_position);
intake_pivot_.set_max_position(max_intake_pivot_position);
+ extend_.set_min_position(min_extend_position);
+ extend_.set_max_position(max_extend_position);
+
// Calculate the loops for a cycle.
const double voltage = intake_pivot_.UpdateController(disabled);
@@ -578,20 +571,9 @@
const flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
intake_pivot_status_offset = intake_pivot_.MakeStatus(status->fbb());
- aos::FlatbufferFixedAllocatorArray<
- frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
- note_goal_buffer;
-
- note_goal_buffer.Finish(
- frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *note_goal_buffer.fbb(), extend_position));
-
- const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
- *note_goal = ¬e_goal_buffer.message();
-
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
extend_status_offset = extend_.Iterate(
- note_goal, position->extend(),
+ extend_goal, position->extend(),
output != nullptr ? &output_struct.extend_voltage : nullptr,
status->fbb());
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 487a073..56452c8 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -416,15 +416,6 @@
superstructure_status_fetcher_->shooter()->turret()->position(),
0.001);
}
- } else if (superstructure_status_fetcher_->uncompleted_note_goal() ==
- NoteStatus::AMP ||
- superstructure_status_fetcher_->uncompleted_note_goal() ==
- NoteStatus::TRAP) {
- EXPECT_NEAR(
- simulated_robot_constants_->common()
- ->turret_avoid_extend_collision_position(),
- superstructure_status_fetcher_->shooter()->turret()->position(),
- 0.001);
}
if (superstructure_goal_fetcher_->has_shooter_goal()) {
@@ -1534,6 +1525,13 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ RunFor(10 * dt());
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+ ExtendStatus::MOVING);
+
RunFor(chrono::seconds(5));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());