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/constants/common.json b/y2024/constants/common.json
index 0fcc969..3a1e1b5 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -234,7 +234,7 @@
},
"extend_set_points": {
"trap": 0.46,
- "amp": 0.2,
+ "amp": 0.35,
"catapult": 0.017,
"retracted": 0.017
},
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) {
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 2571cab..12078ff 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -106,7 +106,7 @@
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,
+ NoteGoal requested_note_goal, flatbuffers::FlatBufferBuilder *fbb,
aos::monotonic_clock::time_point monotonic_now);
private:
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 46cf4c2..e8d40c1 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -564,7 +564,8 @@
extend_goal_position, extend_.estimated_position(),
&max_extend_position, &min_extend_position,
intake_pivot_.estimated_position(), &max_intake_pivot_position,
- &min_intake_pivot_position, status->fbb(), timestamp);
+ &min_intake_pivot_position, requested_note_goal_, status->fbb(),
+ timestamp);
intake_pivot_.set_min_position(min_intake_pivot_position);
intake_pivot_.set_max_position(max_intake_pivot_position);