Amp higher up
Catapult hits the note when you do that, force it to elevate too.
Change-Id: I26800cd0a4532239c60f5c15d598560e82874adf
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index cc00454..2770a4e 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -44,7 +44,7 @@
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,
- flatbuffers::FlatBufferBuilder *fbb,
+ NoteGoal requested_note_goal, flatbuffers::FlatBufferBuilder *fbb,
aos::monotonic_clock::time_point monotonic_now) {
drivetrain_status_fetcher_.Fetch();
@@ -96,8 +96,16 @@
bool aiming = false;
- if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
- (!piece_loaded && state_ == CatapultState::READY)) {
+ if (requested_note_goal == NoteGoal::AMP) {
+ // 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);
+ } 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.
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
turret_goal_builder.get(),
@@ -106,7 +114,6 @@
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
altitude_goal_builder.get(),
robot_constants_->common()->altitude_loading_position());
-
} else {
// We have a game piece, lets start aiming.
if (drivetrain_status_fetcher_.get() != nullptr) {