Superstructure tunings and changes from SFR
Change-Id: I89b801bc37b557630f561993419ad30b8fca484b
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index a36844b..0b3dc47 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -264,6 +264,7 @@
"retracted": 0.017
},
"turret_avoid_extend_collision_position": 0.0,
+ "altitude_avoid_extend_collision_position": 0.3,
"autonomous_mode": "FOUR_PIECE",
"ignore_targets": {
"red": [1, 2, 5, 6, 9, 10, 11, 12, 13, 14, 15, 16],
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index d33a161..c679128 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -194,6 +194,7 @@
// The position to move the turret to when avoiding collision
// with the extend when the extend is moving to amp/trap position.
turret_avoid_extend_collision_position: double (id: 24);
+ altitude_avoid_extend_collision_position: double (id: 28);
autonomous_mode:AutonomousMode (id: 26);
ignore_targets:IgnoreTargets (id: 27);
}
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 7fdf748..4421e15 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -10,7 +10,8 @@
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
-constexpr double kCatapultActivationThreshold = 0.01;
+constexpr double kCatapultActivationTurretThreshold = 0.03;
+constexpr double kCatapultActivationAltitudeThreshold = 0.01;
Shooter::Shooter(aos::EventLoop *event_loop, const Constants *robot_constants)
: drivetrain_status_fetcher_(
@@ -68,6 +69,15 @@
aos::fbs::FixedStackAllocator<aos::fbs::Builder<
frc971::control_loops::
StaticZeroingSingleDOFProfiledSubsystemGoalStatic>::kBufferSize>
+ auto_aim_allocator;
+
+ aos::fbs::Builder<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic>
+ auto_aim_goal_builder(&auto_aim_allocator);
+
+ aos::fbs::FixedStackAllocator<aos::fbs::Builder<
+ frc971::control_loops::
+ StaticZeroingSingleDOFProfiledSubsystemGoalStatic>::kBufferSize>
altitude_allocator;
aos::fbs::Builder<
@@ -96,14 +106,16 @@
bool aiming = false;
- if (requested_note_goal == NoteGoal::AMP) {
+ if (requested_note_goal == NoteGoal::AMP ||
+ requested_note_goal == NoteGoal::TRAP) {
// Being asked to amp, lift the altitude up.
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
turret_goal_builder.get(),
robot_constants_->common()->turret_loading_position());
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
- altitude_goal_builder.get(), 0.3);
+ altitude_goal_builder.get(),
+ robot_constants_->common()->altitude_avoid_extend_collision_position());
} else 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.
@@ -118,15 +130,19 @@
// We have a game piece, lets start aiming.
if (drivetrain_status_fetcher_.get() != nullptr) {
aiming = true;
- aimer_.Update(drivetrain_status_fetcher_.get(),
- frc971::control_loops::aiming::ShotMode::kShootOnTheFly,
- turret_goal_builder.get());
}
}
+ // Auto aim builder is a dummy so we get a status when we aren't aiming.
+ aimer_.Update(
+ drivetrain_status_fetcher_.get(),
+ frc971::control_loops::aiming::ShotMode::kShootOnTheFly,
+ aiming ? turret_goal_builder.get() : auto_aim_goal_builder.get());
+
// We have a game piece and are being asked to aim.
constants::Values::ShotParams shot_params;
- if (piece_loaded && shooter_goal != nullptr && shooter_goal->auto_aim() &&
+ if ((piece_loaded || state_ == CatapultState::FIRING) &&
+ shooter_goal != nullptr && shooter_goal->auto_aim() &&
interpolation_table_.GetInRange(distance_to_goal, &shot_params)) {
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
altitude_goal_builder.get(), shot_params.shot_altitude_angle);
@@ -137,22 +153,24 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*turret_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
+ (piece_loaded || state_ == CatapultState::FIRING) &&
shooter_goal->has_turret_position())
? shooter_goal->turret_position()
: &turret_goal_builder->AsFlatbuffer();
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*altitude_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
+ (piece_loaded || state_ == CatapultState::FIRING) &&
shooter_goal->has_altitude_position())
? shooter_goal->altitude_position()
: &altitude_goal_builder->AsFlatbuffer();
const bool turret_in_range =
(std::abs(turret_.estimated_position() - turret_goal->unsafe_goal()) <
- kCatapultActivationThreshold);
+ kCatapultActivationTurretThreshold);
const bool altitude_in_range =
(std::abs(altitude_.estimated_position() - altitude_goal->unsafe_goal()) <
- kCatapultActivationThreshold);
+ kCatapultActivationAltitudeThreshold);
const bool altitude_above_min_angle =
(altitude_.estimated_position() >
robot_constants_->common()->min_altitude_shooting_angle());
@@ -169,6 +187,20 @@
.extend_position = extend_position},
turret_goal->unsafe_goal(), extend_goal);
+ if (!CatapultRetracted()) {
+ altitude_.set_min_position(
+ robot_constants_->common()->min_altitude_shooting_angle());
+ } else {
+ altitude_.clear_min_position();
+ }
+
+ if (!CatapultRetracted()) {
+ altitude_.set_min_position(
+ robot_constants_->common()->min_altitude_shooting_angle());
+ } else {
+ altitude_.clear_min_position();
+ }
+
turret_.set_min_position(collision_avoidance->min_turret_goal());
turret_.set_max_position(collision_avoidance->max_turret_goal());
@@ -225,8 +257,8 @@
state_ = CatapultState::RETRACTING;
}
- constexpr double kLoadingAcceleration = 20.0;
- constexpr double kLoadingDeceleration = 10.0;
+ constexpr double kLoadingAcceleration = 40.0;
+ constexpr double kLoadingDeceleration = 20.0;
switch (state_) {
case CatapultState::READY:
@@ -271,6 +303,7 @@
catapult_.set_unprofiled_goal(2.0, 0.0);
if (CatapultClose()) {
state_ = CatapultState::RETRACTING;
+ ++shot_count_;
} else {
break;
}
@@ -303,9 +336,7 @@
}
flatbuffers::Offset<AimerStatus> aimer_offset;
- if (aiming) {
- aimer_offset = aimer_.PopulateStatus(fbb);
- }
+ aimer_offset = aimer_.PopulateStatus(fbb);
y2024::control_loops::superstructure::ShooterStatus::Builder status_builder(
*fbb);
@@ -316,9 +347,8 @@
status_builder.add_turret_in_range(turret_in_range);
status_builder.add_altitude_in_range(altitude_in_range);
status_builder.add_altitude_above_min_angle(altitude_above_min_angle);
- if (aiming) {
- status_builder.add_aimer(aimer_offset);
- }
+ status_builder.add_auto_aiming(aiming);
+ status_builder.add_aimer(aimer_offset);
return status_builder.Finish();
}
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 12078ff..a551993 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -109,9 +109,22 @@
NoteGoal requested_note_goal, flatbuffers::FlatBufferBuilder *fbb,
aos::monotonic_clock::time_point monotonic_now);
+ bool loaded() const { return state_ == CatapultState::LOADED; }
+
+ uint32_t shot_count() const { return shot_count_; }
+
+ bool Firing() const {
+ return state_ != CatapultState::READY && state_ != CatapultState::LOADED &&
+ state_ != CatapultState::RETRACTING;
+ }
+
private:
CatapultState state_ = CatapultState::RETRACTING;
+ bool CatapultRetracted() const {
+ return catapult_.estimated_position() < 0.5;
+ }
+
bool CatapultClose() const {
return (std::abs(catapult_.estimated_position() -
catapult_.unprofiled_goal(0, 0)) < 0.05 &&
@@ -138,6 +151,8 @@
interpolation_table_;
Debouncer debouncer_;
+
+ uint32_t shot_count_ = 0;
};
} // namespace y2024::control_loops::superstructure
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 016110a..b30d530 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -16,17 +16,12 @@
// continue.
constexpr double kExtendThreshold = 0.01;
-constexpr double kTurretLoadingThreshold = 0.01;
-constexpr double kAltitudeLoadingThreshold = 0.01;
+constexpr double kTurretLoadingThreshold = 0.05;
+constexpr double kAltitudeLoadingThreshold = 0.02;
constexpr std::chrono::milliseconds kExtraIntakingTime =
std::chrono::milliseconds(500);
-// Exit catapult loading state after this much time if we never
-// trigger any beambreaks.
-constexpr std::chrono::milliseconds kMaxCatapultLoadingTime =
- std::chrono::milliseconds(3000);
-
namespace y2024::control_loops::superstructure {
using ::aos::monotonic_clock;
@@ -54,7 +49,9 @@
shooter_(event_loop, robot_constants_),
extend_(
robot_constants_->common()->extend(),
- robot_constants_->robot()->extend_constants()->zeroing_constants()) {
+ robot_constants_->robot()->extend_constants()->zeroing_constants()),
+ extend_debouncer_(std::chrono::milliseconds(30),
+ std::chrono::milliseconds(8)) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -79,23 +76,45 @@
OutputT output_struct;
+ extend_debouncer_.Update(position->extend_beambreak(), timestamp);
+ const bool extend_beambreak = extend_debouncer_.state();
+
// Handle Climber Goal separately from main superstructure state machine
double climber_position =
robot_constants_->common()->climber_set_points()->retract();
+ double climber_velocity = robot_constants_->common()
+ ->climber()
+ ->default_profile_params()
+ ->max_velocity();
+ const double climber_accel = robot_constants_->common()
+ ->climber()
+ ->default_profile_params()
+ ->max_acceleration();
+
if (unsafe_goal != nullptr) {
switch (unsafe_goal->climber_goal()) {
case ClimberGoal::FULL_EXTEND:
climber_position =
robot_constants_->common()->climber_set_points()->full_extend();
+ // The climber can go reasonably fast when extending out.
+ climber_velocity = 0.5;
+
+ if (unsafe_goal->slow_climber()) {
+ climber_velocity = 0.01;
+ }
break;
case ClimberGoal::RETRACT:
climber_position =
robot_constants_->common()->climber_set_points()->retract();
+ // Keep the climber slower while retracting.
+ climber_velocity = 0.1;
break;
case ClimberGoal::STOWED:
climber_position =
robot_constants_->common()->climber_set_points()->stowed();
+ // Keep the climber slower while retracting.
+ climber_velocity = 0.1;
}
}
@@ -115,17 +134,13 @@
robot_constants_->common()->intake_pivot_set_points()->retracted();
if (unsafe_goal != nullptr) {
- switch (unsafe_goal->intake_goal()) {
- case IntakeGoal::INTAKE:
+ switch (unsafe_goal->intake_pivot()) {
+ case IntakePivotGoal::DOWN:
intake_pivot_position =
robot_constants_->common()->intake_pivot_set_points()->extended();
intake_end_time_ = timestamp;
break;
- case IntakeGoal::SPIT:
- intake_pivot_position =
- robot_constants_->common()->intake_pivot_set_points()->retracted();
- break;
- case IntakeGoal::NONE:
+ case IntakePivotGoal::UP:
intake_pivot_position =
robot_constants_->common()->intake_pivot_set_points()->retracted();
break;
@@ -206,12 +221,11 @@
}
extend_goal_location = ExtendStatus::RETRACTED;
- catapult_requested_ = false;
break;
case SuperstructureState::INTAKING:
// Switch to LOADED state when the extend beambreak is triggered
// meaning the note is loaded in the extend
- if (position->extend_beambreak()) {
+ if (extend_beambreak) {
state_ = SuperstructureState::LOADED;
}
intake_roller_state = IntakeRollerStatus::INTAKING;
@@ -219,11 +233,6 @@
extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_EXTEND;
extend_goal_location = ExtendStatus::RETRACTED;
- if (!catapult_requested_ && unsafe_goal != nullptr &&
- unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
- catapult_requested_ = true;
- }
-
// If we are no longer requesting INTAKE or we are no longer requesting
// an INTAKE goal, wait 0.5 seconds then go back to IDLE.
if (!(unsafe_goal != nullptr &&
@@ -234,7 +243,7 @@
break;
case SuperstructureState::LOADED:
- if (!position->extend_beambreak() && !position->catapult_beambreak()) {
+ if (!extend_beambreak && !position->catapult_beambreak()) {
state_ = SuperstructureState::IDLE;
}
@@ -242,7 +251,7 @@
case NoteGoal::NONE:
break;
case NoteGoal::CATAPULT:
- state_ = SuperstructureState::MOVING;
+ state_ = SuperstructureState::LOADING_CATAPULT;
transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
break;
case NoteGoal::TRAP:
@@ -267,8 +276,8 @@
extend_goal_location = ExtendStatus::CATAPULT;
if (extend_ready_for_catapult_transfer && turret_ready_for_load &&
altitude_ready_for_load) {
- loading_catapult_start_time_ = timestamp;
state_ = SuperstructureState::LOADING_CATAPULT;
+ loading_catapult_start_time_ = timestamp;
}
break;
case NoteGoal::TRAP:
@@ -296,18 +305,22 @@
case SuperstructureState::LOADING_CATAPULT:
extend_moving = false;
extend_goal_location = ExtendStatus::CATAPULT;
- extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_CATAPULT;
- transfer_roller_status = TransferRollerStatus::EXTEND_MOVING;
- // If we lost the game piece, reset state to idle.
- if (((timestamp - loading_catapult_start_time_) >
- kMaxCatapultLoadingTime) &&
- !position->catapult_beambreak() && !position->extend_beambreak()) {
+ if (extend_beambreak) {
+ loading_catapult_start_time_ = timestamp;
+ }
+
+ if (loading_catapult_start_time_ + std::chrono::seconds(10) < timestamp) {
state_ = SuperstructureState::IDLE;
}
+ if (turret_ready_for_load && altitude_ready_for_load) {
+ extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_CATAPULT;
+ transfer_roller_status = TransferRollerStatus::EXTEND_MOVING;
+ }
+
// Switch to READY state when the catapult beambreak is triggered
- if (position->catapult_beambreak()) {
+ if (shooter_.loaded()) {
state_ = SuperstructureState::READY;
}
break;
@@ -327,6 +340,10 @@
break;
case NoteGoal::CATAPULT:
extend_goal_location = ExtendStatus::CATAPULT;
+
+ if (!shooter_.loaded()) {
+ state_ = SuperstructureState::LOADING_CATAPULT;
+ }
break;
case NoteGoal::TRAP:
extend_goal_location = ExtendStatus::TRAP;
@@ -346,14 +363,14 @@
// 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.
- if (!position->catapult_beambreak()) {
+ if (!shooter_.loaded() && !shooter_.Firing()) {
state_ = SuperstructureState::IDLE;
}
break;
case NoteGoal::TRAP:
extend_roller_status = ExtendRollerStatus::SCORING_IN_TRAP;
extend_goal_location = ExtendStatus::TRAP;
- if (!position->extend_beambreak() && unsafe_goal != nullptr &&
+ if (!extend_beambreak && unsafe_goal != nullptr &&
!unsafe_goal->fire()) {
state_ = SuperstructureState::IDLE;
}
@@ -361,7 +378,7 @@
case NoteGoal::AMP:
extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
extend_goal_location = ExtendStatus::AMP;
- if (!position->extend_beambreak() && unsafe_goal != nullptr &&
+ if (!extend_beambreak && unsafe_goal != nullptr &&
!unsafe_goal->fire()) {
state_ = SuperstructureState::IDLE;
}
@@ -374,6 +391,7 @@
unsafe_goal->intake_goal() == IntakeGoal::SPIT) {
intake_roller_state = IntakeRollerStatus::SPITTING;
transfer_roller_status = TransferRollerStatus::TRANSFERING_OUT;
+ extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
}
// Update Intake Roller voltage based on status from state machine.
@@ -441,7 +459,20 @@
double extend_goal_position = 0.0;
+ // If we request trap, override the extend goal to be trap unless we request
+ // amp.
if (unsafe_goal != nullptr && unsafe_goal->note_goal() == NoteGoal::TRAP) {
+ trap_override_ = true;
+ }
+
+ if (unsafe_goal != nullptr && unsafe_goal->note_goal() == NoteGoal::AMP &&
+ trap_override_) {
+ trap_override_ = false;
+ requested_note_goal_ = NoteGoal::AMP;
+ state_ = SuperstructureState::READY;
+ }
+
+ if (trap_override_) {
extend_goal_location = ExtendStatus::TRAP;
}
@@ -504,9 +535,17 @@
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
climber_goal_buffer;
- climber_goal_buffer.Finish(
- frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *climber_goal_buffer.fbb(), climber_position));
+ {
+ flatbuffers::FlatBufferBuilder *climber_fbb = climber_goal_buffer.fbb();
+ flatbuffers::Offset<frc971::ProfileParameters> climber_profile =
+ frc971::CreateProfileParameters(*climber_fbb, climber_velocity,
+ climber_accel);
+
+ climber_goal_buffer.Finish(
+ frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *climber_fbb, climber_position, climber_profile));
+ }
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*climber_goal = &climber_goal_buffer.message();
@@ -619,6 +658,7 @@
status_builder.add_extend_status(extend_status);
status_builder.add_extend(extend_status_offset);
status_builder.add_state(state_);
+ status_builder.add_shot_count(shooter_.shot_count());
status_builder.add_uncompleted_note_goal(uncompleted_note_goal_status);
status_builder.add_extend_ready_for_transfer(extend_at_retracted);
status_builder.add_extend_at_retracted(extend_at_retracted);
@@ -626,7 +666,7 @@
status_builder.add_altitude_ready_for_load(altitude_ready_for_load);
status_builder.add_extend_ready_for_catapult_transfer(
extend_ready_for_catapult_transfer);
- status_builder.add_extend_beambreak(position->extend_beambreak());
+ status_builder.add_extend_beambreak(extend_beambreak);
status_builder.add_catapult_beambreak(position->catapult_beambreak());
(void)status->Send(status_builder.Finish());
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 1c2d119..ce279a6 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -67,12 +67,15 @@
aos::Alliance alliance_ = aos::Alliance::kInvalid;
- bool catapult_requested_ = false;
-
SuperstructureState state_ = SuperstructureState::IDLE;
+ bool trap_override_ = false;
+
NoteGoal requested_note_goal_ = NoteGoal::NONE;
+ aos::monotonic_clock::time_point transfer_start_time_ =
+ aos::monotonic_clock::time_point::min();
+
aos::monotonic_clock::time_point intake_end_time_ =
aos::monotonic_clock::time_point::min();
@@ -85,6 +88,9 @@
Shooter shooter_;
PotAndAbsoluteEncoderSubsystem extend_;
+
+ Debouncer extend_debouncer_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index b96bb65..e2c79d5 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -193,13 +193,9 @@
simulated_robot_constants->common()->catapult()->range())
.middle());
altitude_.InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants->common()->altitude()->range())
- .middle());
+ simulated_robot_constants->common()->altitude_loading_position());
turret_.InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants->common()->turret()->range())
- .middle());
+ simulated_robot_constants->common()->turret_loading_position());
extend_.InitializePosition(
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants->common()->extend()->range())
@@ -392,7 +388,7 @@
double set_point =
superstructure_status_fetcher_->intake_pivot()->goal_position();
- if (superstructure_goal_fetcher_->intake_goal() == IntakeGoal::INTAKE) {
+ if (superstructure_goal_fetcher_->intake_pivot() == IntakePivotGoal::DOWN) {
set_point = simulated_robot_constants_->common()
->intake_pivot_set_points()
->extended();
@@ -421,7 +417,11 @@
if (superstructure_goal_fetcher_->has_shooter_goal()) {
if (superstructure_goal_fetcher_->shooter_goal()
->has_altitude_position() &&
- !superstructure_goal_fetcher_->shooter_goal()->auto_aim()) {
+ !superstructure_goal_fetcher_->shooter_goal()->auto_aim() &&
+ (superstructure_status_fetcher_->uncompleted_note_goal() !=
+ NoteStatus::AMP &&
+ superstructure_status_fetcher_->uncompleted_note_goal() !=
+ NoteStatus::TRAP)) {
EXPECT_NEAR(
superstructure_goal_fetcher_->shooter_goal()
->altitude_position()
@@ -432,6 +432,18 @@
->altitude_position()
->unsafe_goal(),
superstructure_plant_.altitude()->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()
+ ->altitude_avoid_extend_collision_position(),
+ superstructure_status_fetcher_->shooter()->altitude()->position(),
+ 0.001);
+ EXPECT_NEAR(simulated_robot_constants_->common()
+ ->altitude_avoid_extend_collision_position(),
+ superstructure_plant_.altitude()->position(), 0.001);
}
}
@@ -580,31 +592,18 @@
SetEnabled(true);
WaitUntilZeroed();
- superstructure_plant_.turret()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->turret()->range())
- .middle());
- superstructure_plant_.altitude()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .middle());
-
{
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->turret()->range())
- .middle());
+ simulated_robot_constants_->common()->turret_loading_position());
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .middle());
+ simulated_robot_constants_->common()->altitude_loading_position());
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
@@ -620,6 +619,7 @@
goal_builder.add_climber_goal(ClimberGoal::RETRACT);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -636,30 +636,6 @@
// Tests that loops can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
SetEnabled(true);
- superstructure_plant_.intake_pivot()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->intake_pivot()->range())
- .lower);
-
- superstructure_plant_.turret()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->turret()->range())
- .lower);
-
- superstructure_plant_.altitude()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .middle());
-
- superstructure_plant_.climber()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->climber()->range())
- .lower);
-
- superstructure_plant_.extend()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->extend()->range())
- .lower);
WaitUntilZeroed();
{
@@ -685,12 +661,13 @@
shooter_goal_builder.add_turret_position(turret_offset);
shooter_goal_builder.add_altitude_position(altitude_offset);
shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_preloaded(true);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_note_goal(NoteGoal::NONE);
@@ -698,7 +675,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_extend_beambreak(true);
+ superstructure_plant_.set_catapult_beambreak(true);
// Give it a lot of time to get there.
RunFor(chrono::seconds(15));
@@ -706,7 +683,7 @@
VerifyNearGoal();
EXPECT_EQ(superstructure_status_fetcher_->state(),
- SuperstructureState::LOADED);
+ SuperstructureState::READY);
}
// Makes sure that the voltage on a motor is properly pulled back after
@@ -722,17 +699,13 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->turret()->range())
- .upper);
+ *builder.fbb(), simulated_robot_constants_->common()
+ ->turret_avoid_extend_collision_position());
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .upper);
+ *builder.fbb(), simulated_robot_constants_->common()
+ ->altitude_avoid_extend_collision_position());
ShooterGoal::Builder shooter_goal_builder =
builder.MakeBuilder<ShooterGoal>();
@@ -754,7 +727,7 @@
}
superstructure_plant_.set_extend_beambreak(true);
- RunFor(chrono::seconds(20));
+ RunFor(chrono::seconds(30));
VerifyNearGoal();
EXPECT_EQ(superstructure_status_fetcher_->state(),
@@ -776,9 +749,8 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(),
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->altitude()->range())
- .lower,
+ simulated_robot_constants_->common()
+ ->altitude_avoid_extend_collision_position(),
CreateProfileParameters(*builder.fbb(), 20.0, 10));
ShooterGoal::Builder shooter_goal_builder =
@@ -854,6 +826,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -873,6 +846,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::SPIT);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -893,6 +867,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -916,6 +891,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -951,6 +927,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -996,6 +973,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_fire(false);
@@ -1036,6 +1014,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_fire(false);
@@ -1074,6 +1053,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_note_goal(NoteGoal::NONE);
goal_builder.add_fire(false);
@@ -1223,6 +1203,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_shooter_goal(shooter_goal_offset);
goal_builder.add_fire(true);
@@ -1232,7 +1213,7 @@
// Wait until the bot finishes auto-aiming.
WaitUntilNear(kTurretGoal, kAltitudeGoal);
- RunFor(chrono::milliseconds(1000));
+ RunFor(dt());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
@@ -1320,9 +1301,6 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
- LOG(INFO) << EnumNameNoteStatus(
- superstructure_status_fetcher_->uncompleted_note_goal());
-
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::READY);
@@ -1337,7 +1315,14 @@
SetEnabled(true);
WaitUntilZeroed();
- constexpr double kDistanceFromSpeaker = 5.0;
+ constexpr double kDistanceFromSpeaker = 4.0;
+
+ const frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::ShotParams>
+ interpolation_table =
+ y2024::constants::Values::InterpolationTableFromFlatbuffer(
+ simulated_robot_constants_->common()
+ ->shooter_interpolation_table());
const double kRedSpeakerX = simulated_robot_constants_->common()
->shooter_targets()
@@ -1391,6 +1376,11 @@
superstructure_plant_.set_catapult_beambreak(true);
+ constants::Values::ShotParams shot_params;
+
+ EXPECT_TRUE(
+ interpolation_table.GetInRange(kDistanceFromSpeaker, &shot_params));
+
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -1470,6 +1460,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -1496,6 +1487,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::DOWN);
goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -1503,7 +1495,7 @@
superstructure_plant_.set_extend_beambreak(true);
- RunFor(chrono::milliseconds(10));
+ RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -1521,6 +1513,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::AMP);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -1551,6 +1544,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::AMP);
goal_builder.add_fire(true);
@@ -1577,6 +1571,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_intake_pivot(IntakePivotGoal::UP);
goal_builder.add_note_goal(NoteGoal::AMP);
goal_builder.add_fire(false);