Tune 2 ball auto
It now drives back closer to take the shot for accuracy.
Change-Id: I13a9790677230c7e69b0ad303107d5f3d49dacfb
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/actors/auto_splines.cc b/y2022/actors/auto_splines.cc
index 43d49a7..e674dc4 100644
--- a/y2022/actors/auto_splines.cc
+++ b/y2022/actors/auto_splines.cc
@@ -80,13 +80,23 @@
alliance);
}
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineTwoBall(
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineTwoBall1(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance) {
return FixSpline(
builder,
- aos::CopyFlatBuffer<frc971::MultiSpline>(spline_two_, builder->fbb()),
+ aos::CopyFlatBuffer<frc971::MultiSpline>(spline_two1_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineTwoBall2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(spline_two2_, builder->fbb()),
alliance);
}
diff --git a/y2022/actors/auto_splines.h b/y2022/actors/auto_splines.h
index b8db887..2f5c399 100644
--- a/y2022/actors/auto_splines.h
+++ b/y2022/actors/auto_splines.h
@@ -27,8 +27,10 @@
"splines/spline_5_ball_2.json")),
spline_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/spline_5_ball_3.json")),
- spline_two_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline_two_ball.json")) {}
+ spline_two1_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_2_ball.json")),
+ spline_two2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_2_ball_2.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
@@ -53,7 +55,11 @@
*builder,
aos::Alliance alliance);
- flatbuffers::Offset<frc971::MultiSpline> SplineTwoBall(
+ flatbuffers::Offset<frc971::MultiSpline> SplineTwoBall1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineTwoBall2(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance);
@@ -63,7 +69,8 @@
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_1_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_2_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_3_;
- aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_two_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_two1_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_two2_;
};
} // namespace actors
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 0a298d7..4f49f81 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -13,9 +13,9 @@
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
-DEFINE_bool(rapid_react, true,
+DEFINE_bool(rapid_react, false,
"If true, run the main rapid react autonomous mode");
-DEFINE_bool(rapid_react_two, false,
+DEFINE_bool(rapid_react_two, true,
"If true, run the two ball rapid react autonomous mode");
namespace y2022 {
@@ -117,9 +117,12 @@
CHECK(starting_position_);
} else if (FLAGS_rapid_react_two) {
rapid_react_two_spline_ = {
- PlanSpline(std::bind(&AutonomousSplines::SplineTwoBall, &auto_splines_,
+ PlanSpline(std::bind(&AutonomousSplines::SplineTwoBall1, &auto_splines_,
std::placeholders::_1, alliance_),
- SplineDirection::kBackward)};
+ SplineDirection::kBackward),
+ PlanSpline(std::bind(&AutonomousSplines::SplineTwoBall2, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward)};
starting_position_ = rapid_react_two_spline_.value()[0].starting_position();
CHECK(starting_position_);
}
@@ -328,8 +331,14 @@
splines[0].Start();
if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+ std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+ if (!splines[1].WaitForPlan()) return;
+ splines[1].Start();
+ if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
+ std::this_thread::sleep_for(std::chrono::milliseconds(500));
+
// Fire the ball once we stopped
- RetractBackIntake();
set_fire_at_will(true);
SendSuperstructureGoal();
if (!WaitForBallsShot()) return;
@@ -339,6 +348,7 @@
.count()
<< 's';
set_fire_at_will(false);
+ RetractBackIntake();
SendSuperstructureGoal();
}
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index fd5f678..b36e010 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -94,7 +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_;
+ std::optional<std::array<SplineHandle, 2>> 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
index 25f2569..bf7e8be 100644
--- a/y2022/actors/splines/spline_2_ball.json
+++ b/y2022/actors/splines/spline_2_ball.json
@@ -1 +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
+{"spline_count": 1, "spline_x": [1.6215960591132994, 2.0441299469203753, 2.136217765021257, 2.309583831291792, 2.6666670621941027, 2.9165660756303344], "spline_y": [-1.5742995073891626, -1.9839794486639044, -1.8796132548162379, -2.130555402472591, -2.15722202774562, -2.1959456607795707], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file