Autonomous changes for champs
* Add and tune two-piece-via-stage auto
* Tweak five note for champs fields
Change-Id: Ie191bcd4d3a7fe8d3a7f215190e9c84859c43acc
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/autonomous/auto_splines.cc b/y2024/autonomous/auto_splines.cc
index 9e52648..c5fb24c 100644
--- a/y2024/autonomous/auto_splines.cc
+++ b/y2024/autonomous/auto_splines.cc
@@ -223,4 +223,47 @@
alliance);
}
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::TwoPieceViaStageSpline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ two_piece_via_stage_spline_1_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::TwoPieceViaStageSpline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ two_piece_via_stage_spline_2_, builder->fbb()),
+ alliance);
+}
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::TwoPieceViaStageSpline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ two_piece_via_stage_spline_3_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::TwoPieceViaStageSpline4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ two_piece_via_stage_spline_4_, builder->fbb()),
+ alliance);
+}
+
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/auto_splines.h b/y2024/autonomous/auto_splines.h
index f477cee..8ba7f70 100644
--- a/y2024/autonomous/auto_splines.h
+++ b/y2024/autonomous/auto_splines.h
@@ -44,7 +44,19 @@
"splines/2_piece_steal.2.json")),
two_piece_steal_spline_4_(
aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/2_piece_steal.3.json")) {}
+ "splines/2_piece_steal.3.json")),
+ two_piece_via_stage_spline_1_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/2_piece_via_stage.0.json")),
+ two_piece_via_stage_spline_2_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/2_piece_via_stage.1.json")),
+ two_piece_via_stage_spline_3_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/2_piece_via_stage.2.json")),
+ two_piece_via_stage_spline_4_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/2_piece_via_stage.3.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -100,6 +112,23 @@
*builder,
aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> TwoPieceViaStageSpline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> TwoPieceViaStageSpline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> TwoPieceViaStageSpline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> TwoPieceViaStageSpline4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
private:
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> mobility_and_shoot_spline_;
@@ -113,6 +142,15 @@
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> two_piece_steal_spline_2_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> two_piece_steal_spline_3_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> two_piece_steal_spline_4_;
+
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline>
+ two_piece_via_stage_spline_1_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline>
+ two_piece_via_stage_spline_2_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline>
+ two_piece_via_stage_spline_3_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline>
+ two_piece_via_stage_spline_4_;
};
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/autonomous_actor.cc b/y2024/autonomous/autonomous_actor.cc
index 19ff2b8..0f70292 100644
--- a/y2024/autonomous/autonomous_actor.cc
+++ b/y2024/autonomous/autonomous_actor.cc
@@ -129,6 +129,30 @@
two_piece_steal_splines_.value()[0].starting_position();
CHECK(starting_position_);
break;
+ case AutonomousMode::TWO_PIECE_VIA_STAGE:
+ AOS_LOG(INFO, "TWO_PIECE_VIA_STAGE replanning!");
+ two_piece_via_stage_splines_ = {
+ PlanSpline(
+ std::bind(&AutonomousSplines::TwoPieceViaStageSpline1,
+ &auto_splines_, std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::TwoPieceViaStageSpline2,
+ &auto_splines_, std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::TwoPieceViaStageSpline3,
+ &auto_splines_, std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::TwoPieceViaStageSpline4,
+ &auto_splines_, std::placeholders::_1, alliance_),
+ SplineDirection::kBackward)};
+
+ starting_position_ =
+ two_piece_via_stage_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
+ break;
}
is_planned_ = true;
@@ -161,6 +185,9 @@
case AutonomousMode::TWO_PIECE_STEAL:
TwoPieceStealAuto();
break;
+ case AutonomousMode::TWO_PIECE_VIA_STAGE:
+ TwoPieceViaStageAuto();
+ break;
}
return true;
}
@@ -457,6 +484,113 @@
aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
}
+void AutonomousActor::TwoPieceViaStageAuto() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ CHECK(two_piece_via_stage_splines_);
+ auto &splines = *two_piece_via_stage_splines_;
+
+ uint32_t initial_shot_count = shot_count();
+
+ // Always be aiming & firing.
+ Aim();
+ if (!WaitForPreloaded()) return;
+
+ Shoot();
+
+ AOS_LOG(
+ INFO, "Shooting Preloaded Note %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!WaitForNoteFired(initial_shot_count, std::chrono::seconds(2))) return;
+ StopAiming();
+
+ AOS_LOG(
+ INFO, "Shot first note %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ StopFiring();
+
+ AOS_LOG(
+ INFO, "Starting Spline 1 %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!splines[0].WaitForPlan()) return;
+
+ splines[0].Start();
+
+ if (!splines[0].WaitForSplineDistanceRemaining(2.0)) return;
+ Intake();
+
+ if (!splines[0].WaitForSplineDistanceRemaining(0.01)) return;
+
+ if (!splines[1].WaitForPlan()) return;
+
+ AOS_LOG(
+ INFO, "Starting second spline %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ splines[1].Start();
+
+ if (!splines[1].WaitForSplineDistanceRemaining(0.01)) return;
+
+ Aim();
+
+ AOS_LOG(
+ INFO, "Finished second spline %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ std::this_thread::sleep_for(chrono::milliseconds(1000));
+
+ Shoot();
+ StopIntake();
+
+ if (!WaitForNoteFired(initial_shot_count + 1, std::chrono::seconds(2)))
+ return;
+
+ StopFiring();
+ StopAiming();
+
+ AOS_LOG(
+ INFO, "Shot second note, starting drive %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!splines[2].WaitForPlan()) return;
+
+ AOS_LOG(
+ INFO, "Starting third spline %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+ splines[2].Start();
+
+ if (!splines[2].WaitForSplineDistanceRemaining(2.0)) return;
+
+ Intake();
+
+ if (!splines[2].WaitForSplineDistanceRemaining(0.01)) return;
+
+ if (!splines[3].WaitForPlan()) return;
+
+ AOS_LOG(
+ INFO, "Starting fourth spline %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+ splines[3].Start();
+
+ if (!splines[3].WaitForSplineDistanceRemaining(0.01)) return;
+
+ Aim();
+
+ Shoot();
+
+ std::this_thread::sleep_for(chrono::milliseconds(400));
+
+ if (!WaitForNoteFired(initial_shot_count + 2, std::chrono::seconds(2)))
+ return;
+
+ AOS_LOG(
+ INFO, "Done %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+}
+
void AutonomousActor::SendSuperstructureGoal() {
aos::Sender<control_loops::superstructure::GoalStatic>::StaticBuilder
goal_builder = superstructure_goal_sender_.MakeStaticBuilder();
@@ -493,6 +627,11 @@
SendSuperstructureGoal();
}
+void AutonomousActor::StopAiming() {
+ set_auto_aim(control_loops::superstructure::AutoAimMode::NONE);
+ SendSuperstructureGoal();
+}
+
void AutonomousActor::Aim() {
set_auto_aim(control_loops::superstructure::AutoAimMode::SPEAKER);
SendSuperstructureGoal();
diff --git a/y2024/autonomous/autonomous_actor.h b/y2024/autonomous/autonomous_actor.h
index e0b4f33..a126feb 100644
--- a/y2024/autonomous/autonomous_actor.h
+++ b/y2024/autonomous/autonomous_actor.h
@@ -43,11 +43,13 @@
void MobilityAndShoot();
void FourPieceAuto();
void TwoPieceStealAuto();
+ void TwoPieceViaStageAuto();
void SendSuperstructureGoal();
void StopIntake();
void Intake();
void Aim();
+ void StopAiming();
void Shoot();
void StopFiring();
@@ -75,6 +77,7 @@
std::optional<std::array<SplineHandle, 1>> mobility_and_shoot_splines_;
std::optional<std::array<SplineHandle, 5>> four_piece_splines_;
std::optional<std::array<SplineHandle, 4>> two_piece_steal_splines_;
+ std::optional<std::array<SplineHandle, 4>> two_piece_via_stage_splines_;
control_loops::superstructure::IntakeGoal intake_goal_ =
control_loops::superstructure::IntakeGoal::NONE;
diff --git a/y2024/autonomous/splines/2_piece_via_stage.0.json b/y2024/autonomous/splines/2_piece_via_stage.0.json
new file mode 100644
index 0000000..77fa6d1
--- /dev/null
+++ b/y2024/autonomous/splines/2_piece_via_stage.0.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -7.464294557723783,
+ -6.667696122353746,
+ -5.241623212631461,
+ -2.680791277578054,
+ -1.3692863158339523,
+ -0.49099735723340787
+ ],
+ "spline_y": [
+ 0.25504399412981815,
+ -1.1178495529758359,
+ -1.7224603852027636,
+ -2.836160505190026,
+ -2.9055524852010888,
+ -3.119986483935894
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2.0
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 11.0
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/2_piece_via_stage.1.json b/y2024/autonomous/splines/2_piece_via_stage.1.json
new file mode 100644
index 0000000..a8e5b02
--- /dev/null
+++ b/y2024/autonomous/splines/2_piece_via_stage.1.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -0.49099735723340787,
+ -1.811418619189384,
+ -4.027875271387874,
+ -3.9295391727423667,
+ -3.479861068325508,
+ -4.298882384245665
+ ],
+ "spline_y": [
+ -3.119986483935894,
+ -2.797606009442813,
+ -2.6541176112878206,
+ -0.757854688577488,
+ 0.5799550757531173,
+ 0.8135924221044322
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2.5
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 9
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/2_piece_via_stage.2.json b/y2024/autonomous/splines/2_piece_via_stage.2.json
new file mode 100644
index 0000000..100bd6a
--- /dev/null
+++ b/y2024/autonomous/splines/2_piece_via_stage.2.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -4.298882384245665,
+ -3.1646278127814758,
+ -2.4989652488761096,
+ -2.1136673502503305,
+ -1.479573275804385,
+ -0.5811901698817614
+ ],
+ "spline_y": [
+ 0.8135924221044322,
+ 0.4900303572196947,
+ -0.13193291080313996,
+ -0.255438653484485,
+ -0.08108049299548094,
+ 0.03128456186613349
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2.5
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 9
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/2_piece_via_stage.3.json b/y2024/autonomous/splines/2_piece_via_stage.3.json
new file mode 100644
index 0000000..b5f6f36
--- /dev/null
+++ b/y2024/autonomous/splines/2_piece_via_stage.3.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -0.5811901698817614,
+ -1.2747103831333484,
+ -2.2436699872068244,
+ -3.446197110414496,
+ -4.341355458388501,
+ -5.195728191425397
+ ],
+ "spline_y": [
+ 0.03128456186613349,
+ -0.05545731506355678,
+ -0.2142730564625337,
+ 0.5722193927307038,
+ 0.8918482448895331,
+ 0.8868886417599413
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2.5
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 9
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/five_note.3.json b/y2024/autonomous/splines/five_note.3.json
index 89dd72f..1727fee 100644
--- a/y2024/autonomous/splines/five_note.3.json
+++ b/y2024/autonomous/splines/five_note.3.json
@@ -4,17 +4,17 @@
-5.450348048315508,
-5.412173915783321,
-3.905867155254269,
- -2.390277169363972,
- -1.4242176922097336,
- -0.44894876385221405
+ -2.3471594367906876,
+ -1.3810999596364493,
+ -0.4058310312789297
],
"spline_y": [
2.3787148543274097,
2.9222723661220313,
4.035977082209158,
- 2.9014194169393095,
- 2.4045843838581282,
- 2.0389901653192535
+ 2.6411922097100717,
+ 2.1443571766288905,
+ 1.7787629580900157
],
"constraints": [
{
diff --git a/y2024/autonomous/splines/five_note.4.json b/y2024/autonomous/splines/five_note.4.json
index 76ad4c3..e882898 100644
--- a/y2024/autonomous/splines/five_note.4.json
+++ b/y2024/autonomous/splines/five_note.4.json
@@ -1,17 +1,17 @@
{
"spline_count": 1,
"spline_x": [
- -0.44894876385221405,
- -1.42422446903808,
- -2.3902839461923184,
+ -0.4058310312789297,
+ -1.3811067364647958,
+ -2.347166213619034,
-4.170024929006823,
-5.19716914509787,
-5.872082438576494
],
"spline_y": [
- 2.0389901653192535,
- 2.4045869242541067,
- 2.901421957335288,
+ 1.7787629580900157,
+ 2.144359717024869,
+ 2.64119475010605,
2.268920393640828,
1.8811990984641065,
1.7637363058278848