Add SFR 4/5-piece auto
This brings in the 5 piece auto as it existed at the end of SFR.
Note: This squashes multiple commits worth of work.
Change-Id: Ia0bca787647cba9620be40d27d8d1e424d567b03
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/autonomous/BUILD b/y2024/autonomous/BUILD
index f69d9fc..a834f66 100644
--- a/y2024/autonomous/BUILD
+++ b/y2024/autonomous/BUILD
@@ -50,11 +50,13 @@
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/autonomous:user_button_localized_autonomous_actor",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
"//frc971/control_loops/drivetrain:localizer_fbs",
"//y2024:constants",
+ "//y2024/constants:constants_fbs",
"//y2024/control_loops/drivetrain:drivetrain_base",
"//y2024/control_loops/superstructure:superstructure_goal_fbs",
"//y2024/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2024/autonomous/auto_splines.cc b/y2024/autonomous/auto_splines.cc
index 5d2c10f..66b1b28 100644
--- a/y2024/autonomous/auto_splines.cc
+++ b/y2024/autonomous/auto_splines.cc
@@ -18,7 +18,7 @@
// For 2024: The field is mirrored across the center line, and is not
// rotationally symmetric. As such, we only flip the X coordinates when
// changing side of the field.
- if (alliance == aos::Alliance::kBlue) {
+ if (alliance == aos::Alliance::kRed) {
for (size_t ii = 0; ii < spline_x->size(); ++ii) {
spline_x->Mutate(ii, -spline_x->Get(ii));
}
@@ -121,4 +121,63 @@
return FixSpline(builder, multispline_builder.Finish(), alliance);
}
+flatbuffers::Offset<frc971::MultiSpline>
+AutonomousSplines::MobilityAndShootSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ mobility_and_shoot_spline_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_1_, builder->fbb()),
+ alliance);
+}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_2_, builder->fbb()),
+ alliance);
+}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_3_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_4_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FourPieceSpline5(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(
+ four_piece_spline_5_, builder->fbb()),
+ alliance);
+}
+
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/auto_splines.h b/y2024/autonomous/auto_splines.h
index 309222f..93360d3 100644
--- a/y2024/autonomous/auto_splines.h
+++ b/y2024/autonomous/auto_splines.h
@@ -19,7 +19,20 @@
public:
AutonomousSplines()
: test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/test_spline.json")) {}
+ "splines/test_spline.json")),
+ mobility_and_shoot_spline_(
+ aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/mobilityandshoot.0.json")),
+ four_piece_spline_1_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.0.json")),
+ four_piece_spline_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.1.json")),
+ four_piece_spline_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/five_note.2.json")),
+ 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")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -33,9 +46,39 @@
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> MobilityAndShootSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> FourPieceSpline5(
+ 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_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_1_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_2_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_3_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_4_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> four_piece_spline_5_;
};
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/autonomous_actor.cc b/y2024/autonomous/autonomous_actor.cc
index 3c7f2fa..9b4f7f8 100644
--- a/y2024/autonomous/autonomous_actor.cc
+++ b/y2024/autonomous/autonomous_actor.cc
@@ -12,6 +12,7 @@
#include "y2024/control_loops/drivetrain/drivetrain_base.h"
DEFINE_bool(spline_auto, false, "Run simple test S-spline auto mode.");
+DEFINE_bool(do_fifth_piece, true, "");
namespace y2024::autonomous {
@@ -33,7 +34,8 @@
using frc971::control_loops::drivetrain::LocalizerControl;
namespace chrono = ::std::chrono;
-AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
+AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop,
+ const y2024::Constants *robot_constants)
: frc971::autonomous::UserButtonLocalizedAutonomousActor(
event_loop,
control_loops::drivetrain::GetDrivetrainConfig(event_loop)),
@@ -49,16 +51,60 @@
event_loop
->MakeFetcher<::y2024::control_loops::superstructure::Status>(
"/superstructure")),
+ robot_constants_(CHECK_NOTNULL(robot_constants)),
auto_splines_() {}
void AutonomousActor::Replan() {
- if (FLAGS_spline_auto) {
- test_spline_ =
- PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
- std::placeholders::_1, alliance_),
- SplineDirection::kForward);
+ AutonomousMode mode = robot_constants_->common()->autonomous_mode();
+ switch (mode) {
+ case AutonomousMode::NONE:
+ break;
+ case AutonomousMode::SPLINE_AUTO:
+ test_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward);
- starting_position_ = test_spline_->starting_position();
+ starting_position_ = test_spline_->starting_position();
+ break;
+ case AutonomousMode::MOBILITY_AND_SHOOT:
+ AOS_LOG(INFO, "Mobility & shoot replanning!");
+ mobility_and_shoot_splines_ = {PlanSpline(
+ std::bind(&AutonomousSplines::MobilityAndShootSpline, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward)};
+
+ starting_position_ =
+ mobility_and_shoot_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
+ break;
+ case AutonomousMode::FOUR_PIECE:
+ AOS_LOG(INFO, "FOUR_PIECE replanning!");
+ four_piece_splines_ = {
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline1, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline2, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline3, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline4, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::FourPieceSpline5, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward)};
+
+ starting_position_ = four_piece_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
+ break;
}
is_planned_ = true;
@@ -73,10 +119,21 @@
AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
return false;
}
- if (FLAGS_spline_auto) {
- SplineAuto();
- } else {
- AOS_LOG(WARNING, "No auto mode selected.");
+
+ AutonomousMode mode = robot_constants_->common()->autonomous_mode();
+ switch (mode) {
+ case AutonomousMode::NONE:
+ AOS_LOG(WARNING, "No auto mode selected.");
+ break;
+ case AutonomousMode::SPLINE_AUTO:
+ SplineAuto();
+ break;
+ case AutonomousMode::MOBILITY_AND_SHOOT:
+ MobilityAndShoot();
+ break;
+ case AutonomousMode::FOUR_PIECE:
+ FourPieceAuto();
+ break;
}
return true;
}
@@ -118,13 +175,177 @@
if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
}
+void AutonomousActor::MobilityAndShoot() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ uint32_t initial_shot_count = shot_count();
+
+ CHECK(mobility_and_shoot_splines_);
+
+ auto &splines = *mobility_and_shoot_splines_;
+
+ AOS_LOG(INFO, "Going to preload");
+
+ // Always be auto-aiming.
+ Aim();
+
+ if (!WaitForPreloaded()) return;
+
+ AOS_LOG(INFO, "Starting to Move");
+
+ if (!splines[0].WaitForPlan()) return;
+
+ splines[0].Start();
+
+ if (!splines[0].WaitForSplineDistanceRemaining(0.05)) return;
+
+ AOS_LOG(
+ INFO, "Got there %lf s\nNow Shooting\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ Shoot();
+
+ if (!WaitForNoteFired(initial_shot_count, std::chrono::seconds(5))) return;
+
+ StopFiring();
+
+ AOS_LOG(
+ INFO, "Note fired at %lf seconds\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+}
+
+void AutonomousActor::FourPieceAuto() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ CHECK(four_piece_splines_);
+ auto &splines = *four_piece_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;
+
+ AOS_LOG(
+ INFO, "Shot second note, starting drive %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!splines[2].WaitForPlan()) return;
+ splines[2].Start();
+
+ if (!splines[2].WaitForSplineDistanceRemaining(0.01)) return;
+
+ if (!WaitForNoteFired(initial_shot_count + 3, std::chrono::seconds(5)))
+ return;
+
+ AOS_LOG(
+ INFO, "Finished 4 notes at %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!FLAGS_do_fifth_piece) {
+ AOS_LOG(INFO, "Exitting early due to --nodo_fifth_piece");
+ return;
+ }
+
+ if (!splines[3].WaitForPlan()) return;
+ splines[3].Start();
+
+ if (!splines[3].WaitForSplineDistanceRemaining(0.1)) return;
+
+ AOS_LOG(
+ INFO, "Got to 5th note at %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ StopFiring();
+
+ if (!splines[4].WaitForPlan()) return;
+ splines[4].Start();
+
+ if (!splines[4].WaitForSplineDistanceRemaining(0.05)) return;
+
+ AOS_LOG(
+ INFO, "Done with spline %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ std::this_thread::sleep_for(chrono::milliseconds(300));
+
+ AOS_LOG(
+ INFO, "Shooting last note! %lfs\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ Shoot();
+
+ if (!WaitForNoteFired(initial_shot_count + 4, std::chrono::seconds(5)))
+ 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();
goal_builder->set_intake_goal(intake_goal_);
+ if (intake_goal_ == control_loops::superstructure::IntakeGoal::INTAKE) {
+ goal_builder->set_intake_pivot(
+ control_loops::superstructure::IntakePivotGoal::DOWN);
+ } else {
+ goal_builder->set_intake_pivot(
+ control_loops::superstructure::IntakePivotGoal::UP);
+ }
goal_builder->set_note_goal(note_goal_);
goal_builder->set_fire(fire_);
+ goal_builder->set_climber_goal(
+ control_loops::superstructure::ClimberGoal::STOWED);
control_loops::superstructure::ShooterGoalStatic *shooter_goal =
goal_builder->add_shooter_goal();
@@ -151,6 +372,11 @@
SendSuperstructureGoal();
}
+void AutonomousActor::StopFiring() {
+ set_fire(false);
+ SendSuperstructureGoal();
+}
+
[[nodiscard]] bool AutonomousActor::WaitForPreloaded() {
set_preloaded(true);
SendSuperstructureGoal();
@@ -179,4 +405,54 @@
return true;
}
+uint32_t AutonomousActor::shot_count() {
+ superstructure_status_fetcher_.Fetch();
+ return superstructure_status_fetcher_->shot_count();
+}
+
+[[nodiscard]] bool AutonomousActor::WaitForNoteFired(
+ uint32_t penultimate_target_shot_count, std::chrono::nanoseconds timeout) {
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ aos::common::actions::kLoopOffset);
+ aos::monotonic_clock::time_point end_time =
+ event_loop()->monotonic_now() + timeout;
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ phased_loop.SleepUntilNext();
+
+ if (shot_count() > penultimate_target_shot_count ||
+ event_loop()->monotonic_now() > end_time) {
+ return true;
+ }
+ }
+}
+
+[[nodiscard]] bool AutonomousActor::WaitForCatapultReady() {
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ aos::common::actions::kLoopOffset);
+
+ bool loaded = false;
+ while (!loaded) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ phased_loop.SleepUntilNext();
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+
+ loaded = (superstructure_status_fetcher_->state() ==
+ control_loops::superstructure::SuperstructureState::READY);
+ }
+
+ SendSuperstructureGoal();
+
+ return true;
+}
+
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/autonomous_actor.h b/y2024/autonomous/autonomous_actor.h
index 6796c94..fd5038d 100644
--- a/y2024/autonomous/autonomous_actor.h
+++ b/y2024/autonomous/autonomous_actor.h
@@ -8,6 +8,8 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2024/autonomous/auto_splines.h"
+#include "y2024/constants.h"
+#include "y2024/constants/constants_generated.h"
#include "y2024/control_loops/superstructure/superstructure_goal_static.h"
#include "y2024/control_loops/superstructure/superstructure_status_static.h"
@@ -16,7 +18,8 @@
class AutonomousActor
: public ::frc971::autonomous::UserButtonLocalizedAutonomousActor {
public:
- explicit AutonomousActor(::aos::EventLoop *event_loop);
+ explicit AutonomousActor(::aos::EventLoop *event_loop,
+ const y2024::Constants *robot_constants);
private:
void set_intake_goal(control_loops::superstructure::IntakeGoal intake_goal) {
@@ -35,13 +38,21 @@
void Reset() override;
void SplineAuto();
+ void MobilityAndShoot();
+ void FourPieceAuto();
void SendSuperstructureGoal();
void Intake();
void Aim();
void Shoot();
+ void StopFiring();
+
+ uint32_t shot_count();
[[nodiscard]] bool WaitForPreloaded();
+ [[nodiscard]] bool WaitForNoteFired(uint32_t penultimate_target_shot_count,
+ std::chrono::nanoseconds timeout);
+ [[nodiscard]] bool WaitForCatapultReady();
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
@@ -52,15 +63,19 @@
aos::Fetcher<y2024::control_loops::superstructure::Status>
superstructure_status_fetcher_;
+ const Constants *robot_constants_;
+
AutonomousSplines auto_splines_;
std::optional<SplineHandle> test_spline_;
+ std::optional<std::array<SplineHandle, 1>> mobility_and_shoot_splines_;
+ std::optional<std::array<SplineHandle, 5>> four_piece_splines_;
control_loops::superstructure::IntakeGoal intake_goal_ =
control_loops::superstructure::IntakeGoal::NONE;
control_loops::superstructure::NoteGoal note_goal_ =
- control_loops::superstructure::NoteGoal::NONE;
+ control_loops::superstructure::NoteGoal::CATAPULT;
bool auto_aim_ = false;
bool fire_ = false;
@@ -69,4 +84,4 @@
} // namespace y2024::autonomous
-#endif // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
\ No newline at end of file
+#endif // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
diff --git a/y2024/autonomous/autonomous_actor_main.cc b/y2024/autonomous/autonomous_actor_main.cc
index 4a8e146..5049b0f 100644
--- a/y2024/autonomous/autonomous_actor_main.cc
+++ b/y2024/autonomous/autonomous_actor_main.cc
@@ -2,6 +2,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "y2024/autonomous/autonomous_actor.h"
int main(int argc, char *argv[]) {
@@ -10,8 +11,16 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig("aos_config.json");
+ frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+ ::aos::ShmEventLoop constant_fetcher_event_loop(&config.message());
+ frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher(
+ &constant_fetcher_event_loop);
+
+ const y2024::Constants *robot_constants = &constants_fetcher.constants();
+
::aos::ShmEventLoop event_loop(&config.message());
- ::y2024::autonomous::AutonomousActor autonomous(&event_loop);
+ ::y2024::autonomous::AutonomousActor autonomous(&event_loop, robot_constants);
event_loop.Run();
diff --git a/y2024/autonomous/splines/five_note.0.json b/y2024/autonomous/splines/five_note.0.json
new file mode 100644
index 0000000..ba8ac2c
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.0.json
@@ -0,0 +1,33 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -6.96,
+ -6.616444808957066,
+ -6.440474125870341,
+ -6.410863453750628,
+ -6.106827763281826,
+ -5.963275576067123
+ ],
+ "spline_y": [
+ 0.474,
+ 0.474,
+ 0.3795291901917933,
+ 0.4482348806874367,
+ 0.31444590142149437,
+ 0.26227873892724274
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 4.0
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/five_note.1.json b/y2024/autonomous/splines/five_note.1.json
new file mode 100644
index 0000000..d65db75
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.1.json
@@ -0,0 +1,39 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -5.963275576067123,
+ -6.043899374861988,
+ -6.14218730470054,
+ -6.299979097775238,
+ -6.333362095101602,
+ -6.385753156016853
+ ],
+ "spline_y": [
+ 0.26227873892724274,
+ 0.2915775945855339,
+ 0.27680408013743,
+ 0.19285548109740952,
+ 0.09423634731960748,
+ -0.012742650262493882
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ },
+ {
+ "constraint_type": "VELOCITY",
+ "value": 0.9,
+ "start_distance": 0.0,
+ "end_distance": 10.0
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/five_note.2.json b/y2024/autonomous/splines/five_note.2.json
new file mode 100644
index 0000000..e67e8f0
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.2.json
@@ -0,0 +1,39 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -6.385753156016853,
+ -6.038148386644435,
+ -5.498099209905131,
+ -5.500010615292536,
+ -5.505673744840998,
+ -5.450725524128315
+ ],
+ "spline_y": [
+ -0.012742650262493882,
+ 0.6970427431345343,
+ 1.0584941253106013,
+ 1.0226571920273129,
+ 1.4628922679287215,
+ 2.245294280677646
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 3
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 2
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10
+ },
+ {
+ "constraint_type": "VELOCITY",
+ "value": 0.4,
+ "start_distance": 1.5,
+ "end_distance": 10.0
+ }
+ ]
+}
\ No newline at end of file
diff --git a/y2024/autonomous/splines/five_note.3.json b/y2024/autonomous/splines/five_note.3.json
new file mode 100644
index 0000000..148c79d
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.3.json
@@ -0,0 +1,39 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -5.450725524128315,
+ -5.4125513915961285,
+ -3.9062446310670764,
+ -2.1352203510523458,
+ -1.1691608738981074,
+ -0.19389194554058786
+ ],
+ "spline_y": [
+ 2.245294280677646,
+ 2.7888517924722676,
+ 3.9025565085593934,
+ 2.6507161302136577,
+ 2.1538810971324764,
+ 1.7882868785936012
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 4.5
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 4.0
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10.0
+ },
+ {
+ "constraint_type": "VELOCITY",
+ "value": 6.4,
+ "start_distance": 0.0,
+ "end_distance": 100.0
+ }
+ ]
+}
diff --git a/y2024/autonomous/splines/five_note.4.json b/y2024/autonomous/splines/five_note.4.json
new file mode 100644
index 0000000..93421e2
--- /dev/null
+++ b/y2024/autonomous/splines/five_note.4.json
@@ -0,0 +1,39 @@
+{
+ "spline_count": 1,
+ "spline_x": [
+ -0.19389194554058786,
+ -1.169167650726454,
+ -2.1352271278806922,
+ -3.6552329072641223,
+ -4.682377123355169,
+ -5.357290416833793
+ ],
+ "spline_y": [
+ 1.7882868785936012,
+ 2.153883637528455,
+ 2.650718670609636,
+ 2.3043000750345737,
+ 1.9165787798578524,
+ 1.7991159872216307
+ ],
+ "constraints": [
+ {
+ "constraint_type": "LONGITUDINAL_ACCELERATION",
+ "value": 4.5
+ },
+ {
+ "constraint_type": "LATERAL_ACCELERATION",
+ "value": 4.0
+ },
+ {
+ "constraint_type": "VOLTAGE",
+ "value": 10.000000000000007
+ },
+ {
+ "constraint_type": "VELOCITY",
+ "value": 6.4,
+ "start_distance": 0.0,
+ "end_distance": 100.0
+ }
+ ]
+}
diff --git a/y2024/autonomous/splines/mobilityandshoot.0.json b/y2024/autonomous/splines/mobilityandshoot.0.json
new file mode 100644
index 0000000..22a44cf
--- /dev/null
+++ b/y2024/autonomous/splines/mobilityandshoot.0.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-7.7, -7.184654377431906, -6.673491731517509, -6.769334727626459, -6.098433754863813, -5.6], "spline_y": [-0.9, -0.9, -0.9, -0.9, -0.9, -0.9], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 4.000000000000021}, {"constraint_type": "VELOCITY", "value": 1.0}]}
diff --git a/y2024/autonomous/splines/test_spline.json b/y2024/autonomous/splines/test_spline.json
new file mode 100644
index 0000000..733d516
--- /dev/null
+++ b/y2024/autonomous/splines/test_spline.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}