Merge "5 ball auto for SFR"
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}]}