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/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();