5 ball auto for SFR
1. Shoot the first ball at the start, and drive in a half circle
like motion picking up the 2 balls in the top right of the field,
returning to around the same shot distance in the starting zone
and shoot the 2 balls we have
2. Drive straight back to the human player station and pick up the
ball that is laying there and pick one up from the human player
3. Drive back to the same position it was in just before and shoot
The drive time of this auto is around 10.6 seconds.
Keep in mind that number is without the time to shoot.
Change-Id: I2eef6b9a881951756cee771125f064024547cc18
Signed-off-by: Henry Speiser <henry@speiser.net>
diff --git a/y2022/actors/auto_splines.cc b/y2022/actors/auto_splines.cc
index b06ec99..9dd1860 100644
--- a/y2022/actors/auto_splines.cc
+++ b/y2022/actors/auto_splines.cc
@@ -80,25 +80,5 @@
alliance);
}
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::Spline4(
- aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder,
- aos::Alliance alliance) {
- return FixSpline(
- builder,
- aos::CopyFlatBuffer<frc971::MultiSpline>(spline_4_, builder->fbb()),
- alliance);
-}
-
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::Spline5(
- aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder,
- aos::Alliance alliance) {
- return FixSpline(
- builder,
- aos::CopyFlatBuffer<frc971::MultiSpline>(spline_5_, builder->fbb()),
- alliance);
-}
-
} // namespace actors
} // namespace y2022
diff --git a/y2022/actors/auto_splines.h b/y2022/actors/auto_splines.h
index 4d532f4..546710c 100644
--- a/y2022/actors/auto_splines.h
+++ b/y2022/actors/auto_splines.h
@@ -22,15 +22,11 @@
: test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/test_spline.json")),
spline_1_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline_1.json")),
+ "splines/spline_5_ball_1.json")),
spline_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline_2.json")),
+ "splines/spline_5_ball_2.json")),
spline_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline_3.json")),
- spline_4_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline_4.json")),
- spline_5_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline_5.json")) {}
+ "splines/spline_5_ball_3.json")){}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
@@ -54,22 +50,12 @@
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance);
- flatbuffers::Offset<frc971::MultiSpline> Spline4(
- aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder,
- aos::Alliance alliance);
- flatbuffers::Offset<frc971::MultiSpline> Spline5(
- 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_4_;
- aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_5_;
};
} // namespace actors
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 339d7bf..5e344ac 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -104,18 +104,12 @@
rapid_react_splines_ = {
PlanSpline(std::bind(&AutonomousSplines::Spline1, &auto_splines_,
std::placeholders::_1, alliance_),
- SplineDirection::kForward),
+ SplineDirection::kBackward),
PlanSpline(std::bind(&AutonomousSplines::Spline2, &auto_splines_,
std::placeholders::_1, alliance_),
SplineDirection::kForward),
PlanSpline(std::bind(&AutonomousSplines::Spline3, &auto_splines_,
std::placeholders::_1, alliance_),
- SplineDirection::kForward),
- PlanSpline(std::bind(&AutonomousSplines::Spline4, &auto_splines_,
- std::placeholders::_1, alliance_),
- SplineDirection::kBackward),
- PlanSpline(std::bind(&AutonomousSplines::Spline5, &auto_splines_,
- std::placeholders::_1, alliance_),
SplineDirection::kBackward)};
starting_position_ = rapid_react_splines_.value()[0].starting_position();
CHECK(starting_position_);
@@ -212,56 +206,53 @@
auto &splines = *rapid_react_splines_;
// Tell the superstructure a ball was preloaded
-
if (!WaitForPreloaded()) return;
- // Drive and intake the 2nd ball
- ExtendFrontIntake();
+
+ // Fire preloaded ball
+ set_turret_goal(constants::Values::kTurretFrontIntakePos());
+ set_fire_at_will(true);
+ SendSuperstructureGoal();
+ if (!WaitForBallsShot(1)) return;
+ set_fire_at_will(false);
+ SendSuperstructureGoal();
+
+ // Drive and intake the 2 balls in nearest to the starting zonei
+ set_turret_goal(constants::Values::kTurretBackIntakePos());
+ ExtendBackIntake();
if (!splines[0].WaitForPlan()) return;
splines[0].Start();
if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
// Fire the two balls once we stopped
+ RetractBackIntake();
set_fire_at_will(true);
SendSuperstructureGoal();
if (!WaitForBallsShot(2)) return;
set_fire_at_will(false);
-
- // Drive and intake the 3rd ball
- if (!splines[1].WaitForPlan()) return;
- splines[1].Start();
- if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
-
- // Fire the 3rd once we stopped.
- set_fire_at_will(true);
SendSuperstructureGoal();
- if (!WaitForBallsShot(1)) return;
- set_fire_at_will(false);
// Drive to the human player station while intaking two balls.
// Once is already placed down,
// and one will be rolled to the robot by the human player
- if (!splines[2].WaitForPlan()) return;
- splines[2].Start();
- if (!splines[2].WaitForSplineDistanceRemaining(0.02)) return;
+ ExtendFrontIntake();
+ if (!splines[1].WaitForPlan()) return;
+ splines[1].Start();
+ if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
// Drive to the shooting position
- if (!splines[3].WaitForPlan()) return;
- splines[3].Start();
- if (!splines[3].WaitForSplineDistanceRemaining(0.02)) return;
+ if (!splines[2].WaitForPlan()) return;
+ splines[2].Start();
+ if (!splines[2].WaitForSplineDistanceRemaining(2.00)) return;
+ RetractFrontIntake();
+
+ if (!splines[2].WaitForSplineDistanceRemaining(0.02)) return;
// Fire the two balls once we stopped
set_fire_at_will(true);
SendSuperstructureGoal();
if (!WaitForBallsShot(2)) return;
set_fire_at_will(false);
-
- // Done intaking
- RetractFrontIntake();
-
- // Drive to the middle of the field to get ready for teleop
- if (!splines[4].WaitForPlan()) return;
- splines[4].Start();
- if (!splines[4].WaitForSplineDistanceRemaining(0.02)) return;
+ SendSuperstructureGoal();
LOG(INFO) << "Took "
<< chrono::duration<double>(aos::monotonic_clock::now() -
@@ -312,6 +303,11 @@
CreateProfileParameters(*builder.fbb(), 20.0, 60.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), turret_goal_,
+ CreateProfileParameters(*builder.fbb(), 12.0, 20.0));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
catapult_return_position_offset =
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), kCatapultReturnPosition,
@@ -339,10 +335,11 @@
transfer_roller_front_voltage_);
superstructure_builder.add_transfer_roller_speed_back(
transfer_roller_back_voltage_);
+ superstructure_builder.add_turret(turret_offset);
superstructure_builder.add_catapult(catapult_goal_offset);
superstructure_builder.add_fire(fire_);
superstructure_builder.add_preloaded(preloaded_);
- superstructure_builder.add_auto_aim(true);
+ superstructure_builder.add_auto_aim(false);
if (builder.Send(superstructure_builder.Finish()) !=
aos::RawSender::Error::kOk) {
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index b7d89bf..461cfc1 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -50,6 +50,9 @@
void set_requested_intake(std::optional<RequestedIntake> requested_intake) {
requested_intake_ = requested_intake;
}
+ void set_turret_goal(double turret_goal) {
+ turret_goal_ = turret_goal;
+ }
void set_fire_at_will(bool fire) { fire_ = fire; }
void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
@@ -80,6 +83,7 @@
double transfer_roller_front_voltage_ = 0.0;
double transfer_roller_back_voltage_ = 0.0;
std::optional<RequestedIntake> requested_intake_ = std::nullopt;
+ double turret_goal_ = 0.0;
bool fire_ = false;
bool preloaded_ = false;
@@ -96,7 +100,7 @@
aos::TimerHandler *button_poll_;
std::optional<SplineHandle> test_spline_;
- std::optional<std::array<SplineHandle, 5>> rapid_react_splines_;
+ std::optional<std::array<SplineHandle, 3>> rapid_react_splines_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
AutonomousSplines auto_splines_;
diff --git a/y2022/actors/splines/spline_5_ball_1.json b/y2022/actors/splines/spline_5_ball_1.json
new file mode 100644
index 0000000..03b5644
--- /dev/null
+++ b/y2022/actors/splines/spline_5_ball_1.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-0.18145693702491972, -0.20576409082414582, 0.49914336935341463, 5.522928566665585, 2.880988556589501, 1.845502911614626], "spline_y": [2.346189480782648, 3.695236516639704, 4.837672745203337, 2.7973591802504263, 2.3618745632049176, 1.471550457245212], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2022/actors/splines/spline_5_ball_2.json b/y2022/actors/splines/spline_5_ball_2.json
new file mode 100644
index 0000000..975d6ed
--- /dev/null
+++ b/y2022/actors/splines/spline_5_ball_2.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [1.845502911614626, 2.4171345421461163, 3.004385177342393, 5.2666273627598565, 5.91167736359207, 6.7133296469650885], "spline_y": [1.471550457245212, 2.0368843425108123, 1.4829129663110887, 1.7324754229926884, 1.9201953550863622, 2.6087941113170316], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
diff --git a/y2022/actors/splines/spline_5_ball_3.json b/y2022/actors/splines/spline_5_ball_3.json
new file mode 100644
index 0000000..e9e1a41
--- /dev/null
+++ b/y2022/actors/splines/spline_5_ball_3.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [6.7133296469650885, 6.343336285408311, 5.737641975476239, 3.4208648418667504, 2.51547308321015, 1.841750386467515], "spline_y": [2.6087941113170316, 2.2490783431368313, 1.931858867758041, 1.9600940870614552, 2.031396541720077, 1.4679812465169668], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}