Add a three piece auto which takes center notes
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ib8f263e81bbf89e0d4fcf8d4248f63f1e41e7e39
diff --git a/y2024/autonomous/auto_splines.cc b/y2024/autonomous/auto_splines.cc
index 66b1b28..9e52648 100644
--- a/y2024/autonomous/auto_splines.cc
+++ b/y2024/autonomous/auto_splines.cc
@@ -180,4 +180,47 @@
alliance);
}
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::TwoPieceStealSpline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ two_piece_steal_spline_1_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::TwoPieceStealSpline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ two_piece_steal_spline_2_, builder->fbb()),
+ alliance);
+}
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::TwoPieceStealSpline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ two_piece_steal_spline_3_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::TwoPieceStealSpline4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ two_piece_steal_spline_4_, builder->fbb()),
+ alliance);
+}
+
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/auto_splines.h b/y2024/autonomous/auto_splines.h
index 93360d3..8eb38e8 100644
--- a/y2024/autonomous/auto_splines.h
+++ b/y2024/autonomous/auto_splines.h
@@ -32,7 +32,19 @@
four_piece_spline_4_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/five_note.3.json")),
four_piece_spline_5_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/five_note.4.json")) {}
+ "splines/five_note.4.json")),
+ two_piece_steal_spline_1_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.0.json")),
+ two_piece_steal_spline_2_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.1.json")),
+ two_piece_steal_spline_3_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.2.json")),
+ two_piece_steal_spline_4_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.3.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -71,6 +83,23 @@
*builder,
aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> TwoPieceStealSpline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> TwoPieceStealSpline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> TwoPieceStealSpline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> TwoPieceStealSpline4(
+ 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_;
@@ -79,6 +108,11 @@
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_3_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_4_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_5_;
+
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> two_piece_steal_spline_1_;
+ 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_;
};
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/autonomous_actor.cc b/y2024/autonomous/autonomous_actor.cc
index 9b4f7f8..f50e8f6 100644
--- a/y2024/autonomous/autonomous_actor.cc
+++ b/y2024/autonomous/autonomous_actor.cc
@@ -105,6 +105,25 @@
starting_position_ = four_piece_splines_.value()[0].starting_position();
CHECK(starting_position_);
break;
+ case AutonomousMode::TWO_PIECE_STEAL:
+ AOS_LOG(INFO, "TWO_PIECE_STEAL replanning!");
+ two_piece_steal_splines_ = {
+ PlanSpline(
+ std::bind(&AutonomousSplines::TwoPieceStealSpline1,
+ &auto_splines_, std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::TwoPieceStealSpline2,
+ &auto_splines_, std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::TwoPieceStealSpline3,
+ &auto_splines_, std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::TwoPieceStealSpline4,
+ &auto_splines_, std::placeholders::_1, alliance_),
+ SplineDirection::kForward)};
}
is_planned_ = true;
@@ -134,6 +153,9 @@
case AutonomousMode::FOUR_PIECE:
FourPieceAuto();
break;
+ case AutonomousMode::TWO_PIECE_STEAL:
+ TwoPieceStealAuto();
+ break;
}
return true;
}
@@ -330,6 +352,99 @@
aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
}
+void AutonomousActor::TwoPieceStealAuto() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ CHECK(two_piece_steal_splines_);
+ auto &splines = *two_piece_steal_splines_;
+
+ uint32_t initial_shot_count = shot_count();
+
+ // Always be aiming & firing.
+ Aim();
+ if (!WaitForPreloaded()) return;
+
+ std::this_thread::sleep_for(chrono::milliseconds(500));
+ 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;
+
+ AOS_LOG(
+ INFO, "Shot first note %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ Intake();
+ 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(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;
+
+ AOS_LOG(
+ INFO, "Finished second spline %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ std::this_thread::sleep_for(chrono::milliseconds(250));
+
+ Shoot();
+
+ if (!WaitForNoteFired(initial_shot_count + 1, std::chrono::seconds(2)))
+ return;
+
+ StopFiring();
+
+ 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(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;
+
+ Shoot();
+
+ 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();
diff --git a/y2024/autonomous/autonomous_actor.h b/y2024/autonomous/autonomous_actor.h
index fd5038d..0de85bf 100644
--- a/y2024/autonomous/autonomous_actor.h
+++ b/y2024/autonomous/autonomous_actor.h
@@ -40,6 +40,7 @@
void SplineAuto();
void MobilityAndShoot();
void FourPieceAuto();
+ void TwoPieceStealAuto();
void SendSuperstructureGoal();
void Intake();
@@ -70,6 +71,7 @@
std::optional<SplineHandle> test_spline_;
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_;
control_loops::superstructure::IntakeGoal intake_goal_ =
control_loops::superstructure::IntakeGoal::NONE;
diff --git a/y2024/autonomous/splines/2_piece_steal.0.json b/y2024/autonomous/splines/2_piece_steal.0.json
new file mode 100644
index 0000000..e7f2843
--- /dev/null
+++ b/y2024/autonomous/splines/2_piece_steal.0.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -6.860785213920586,
+ -5.203804609147312,
+ -3.7777316994250265,
+ -2.667592923096702,
+ -1.3560879613526007,
+ -0.5092457179511491
+ ],
+ "spline_y": [
+ -1.1157080654374518,
+ -1.1036636998527491,
+ -1.7082745320796768,
+ -2.823974049798384,
+ -2.893366029809447,
+ -3.209959853823472
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/2_piece_steal.1.json b/y2024/autonomous/splines/2_piece_steal.1.json
new file mode 100644
index 0000000..98c1323
--- /dev/null
+++ b/y2024/autonomous/splines/2_piece_steal.1.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -0.5092457179511491,
+ -1.8931992589519813,
+ -3.6060816001326397,
+ -3.8917290706695438,
+ -4.713201267483203,
+ -5.640902947694428
+ ],
+ "spline_y": [
+ -3.209959853823472,
+ -2.6925657969997197,
+ -2.333279192015276,
+ -2.186602322153238,
+ -1.549542211476115,
+ -0.8645502069901454
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/2_piece_steal.2.json b/y2024/autonomous/splines/2_piece_steal.2.json
new file mode 100644
index 0000000..ef955db
--- /dev/null
+++ b/y2024/autonomous/splines/2_piece_steal.2.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -5.640902947694428,
+ -4.662999278601822,
+ -3.239591234160512,
+ -3.047507215729768,
+ -1.061657126562114,
+ -0.47709853308607464
+ ],
+ "spline_y": [
+ -0.8645502069901454,
+ -1.5866101199714517,
+ -3.0049314806309253,
+ -3.286832304945575,
+ -2.0927386168786777,
+ -1.9056608123457113
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/2_piece_steal.3.json b/y2024/autonomous/splines/2_piece_steal.3.json
new file mode 100644
index 0000000..356d7e9
--- /dev/null
+++ b/y2024/autonomous/splines/2_piece_steal.3.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -0.47709853308607464,
+ -1.1147335735554593,
+ -3.3428566977932013,
+ -2.7197061644307787,
+ -4.462847247516866,
+ -5.662538809543391
+ ],
+ "spline_y": [
+ -1.9056608123457113,
+ -2.1097248100280774,
+ -2.6177296382002626,
+ -2.8074138647816946,
+ -1.752456754116137,
+ -0.8676468782753499
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ }
+ ]
+}
\ No newline at end of file