Two ball auto rapid react
Signed-off-by: Henry Speiser <henry@speiser.net>
Change-Id: I0600cebeab736098c6f58911816efded09eab10e
diff --git a/y2022/actors/auto_splines.cc b/y2022/actors/auto_splines.cc
index 9dd1860..43d49a7 100644
--- a/y2022/actors/auto_splines.cc
+++ b/y2022/actors/auto_splines.cc
@@ -80,5 +80,15 @@
alliance);
}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineTwoBall(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(spline_two_, builder->fbb()),
+ alliance);
+}
+
} // namespace actors
} // namespace y2022
diff --git a/y2022/actors/auto_splines.h b/y2022/actors/auto_splines.h
index 546710c..b8db887 100644
--- a/y2022/actors/auto_splines.h
+++ b/y2022/actors/auto_splines.h
@@ -26,7 +26,9 @@
spline_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/spline_5_ball_2.json")),
spline_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline_5_ball_3.json")){}
+ "splines/spline_5_ball_3.json")),
+ spline_two_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_two_ball.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
@@ -51,11 +53,17 @@
*builder,
aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineTwoBall(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
private:
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_1_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_2_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_3_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_two_;
};
} // namespace actors
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index ca109e0..0a298d7 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -15,6 +15,8 @@
DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
DEFINE_bool(rapid_react, true,
"If true, run the main rapid react autonomous mode");
+DEFINE_bool(rapid_react_two, false,
+ "If true, run the two ball rapid react autonomous mode");
namespace y2022 {
namespace actors {
@@ -113,6 +115,13 @@
SplineDirection::kForward)};
starting_position_ = rapid_react_splines_.value()[0].starting_position();
CHECK(starting_position_);
+ } else if (FLAGS_rapid_react_two) {
+ rapid_react_two_spline_ = {
+ PlanSpline(std::bind(&AutonomousSplines::SplineTwoBall, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward)};
+ starting_position_ = rapid_react_two_spline_.value()[0].starting_position();
+ CHECK(starting_position_);
}
is_planned_ = true;
@@ -163,6 +172,8 @@
SplineAuto();
} else if (FLAGS_rapid_react) {
RapidReact();
+ } else if (FLAGS_rapid_react_two) {
+ RapidReactTwo();
}
return true;
@@ -291,6 +302,46 @@
<< 's';
}
+// Rapid React Two Ball Autonomous.
+void AutonomousActor::RapidReactTwo() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ CHECK(rapid_react_two_spline_);
+
+ auto &splines = *rapid_react_two_spline_;
+
+ // Tell the superstructure a ball was preloaded
+ if (!WaitForPreloaded()) return;
+ set_fire_at_will(true);
+ SendSuperstructureGoal();
+ if (!WaitForBallsShot()) return;
+ LOG(INFO) << "Shot first ball "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count()
+ << 's';
+ set_fire_at_will(false);
+ SendSuperstructureGoal();
+
+ ExtendBackIntake();
+ if (!splines[0].WaitForPlan()) return;
+ splines[0].Start();
+ if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+
+ // Fire the ball once we stopped
+ RetractBackIntake();
+ set_fire_at_will(true);
+ SendSuperstructureGoal();
+ if (!WaitForBallsShot()) return;
+ LOG(INFO) << "Shot last ball "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count()
+ << 's';
+ set_fire_at_will(false);
+ SendSuperstructureGoal();
+}
+
[[nodiscard]] bool AutonomousActor::WaitForPreloaded() {
set_preloaded(true);
SendSuperstructureGoal();
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index 0cf8c6d..fd5f678 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -67,6 +67,7 @@
void SplineAuto();
void RapidReact();
+ void RapidReactTwo();
void Replan();
@@ -93,6 +94,7 @@
std::optional<SplineHandle> test_spline_;
std::optional<std::array<SplineHandle, 3>> rapid_react_splines_;
+ std::optional<std::array<SplineHandle, 1>> rapid_react_two_spline_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
AutonomousSplines auto_splines_;
diff --git a/y2022/actors/splines/spline_2_ball.json b/y2022/actors/splines/spline_2_ball.json
new file mode 100644
index 0000000..25f2569
--- /dev/null
+++ b/y2022/actors/splines/spline_2_ball.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [1.6215960591132994, 2.0441299469203753, 2.136217765021257, 2.3790533454759926, 2.694613481030699, 2.970665105357857], "spline_y": [-1.5742995073891626, -1.9839794486639044, -1.8796132548162379, -2.0078020021461, -2.061911466549475, -2.069798655815965], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file