Add fender auto and auto-select it
Auto now automatically selects fender vs aligned depending on if we are
971 or 9971.
Change-Id: Ic6bd0b10af9049fba01eba25f29c383c9d3b4a2b
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 48da130..0ed59e2 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -151,6 +151,22 @@
alliance);
}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarSideFender(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ // I drew the spline on the wrong side of the field.
+ if (alliance == aos::Alliance::kBlue) {
+ alliance = aos::Alliance::kRed;
+ } else {
+ alliance = aos::Alliance::kBlue;
+ }
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(far_side_fender_,
+ builder->fbb()),
+ alliance);
+}
+
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) {
diff --git a/y2020/actors/auto_splines.h b/y2020/actors/auto_splines.h
index 2dd99b7..e7bef0a 100644
--- a/y2020/actors/auto_splines.h
+++ b/y2020/actors/auto_splines.h
@@ -30,7 +30,9 @@
target_offset_1_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/target_offset_1.json")),
target_offset_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/target_offset_2.json")) {}
+ "splines/target_offset_2.json")),
+ far_side_fender_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/9971_to_fender.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
@@ -72,6 +74,11 @@
builder->fbb());
}
+ flatbuffers::Offset<frc971::MultiSpline> FarSideFender(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
private:
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> target_aligned_1_;
@@ -79,6 +86,7 @@
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> target_aligned_3_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> target_offset_1_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> target_offset_2_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> far_side_fender_;
};
} // namespace actors
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index c8f1ebe..7aa649a 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -5,18 +5,14 @@
#include <cmath>
#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
#include "aos/util/math.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2020/actors/auto_splines.h"
+#include "y2020/constants.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
-DEFINE_bool(target_aligned, true,
- "If true, run the Infinite Recharge autonomous that starts aligned "
- "with the target");
-DEFINE_bool(target_offset, false,
- "If true, run the Infinite Recharge autonomous that starts offset "
- "from the target");
DEFINE_bool(just_shoot, false,
"If true, run the autonomous that just shoots balls.");
@@ -45,6 +41,9 @@
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
auto_splines_() {
+ practice_robot_ =
+ ::aos::network::GetTeamNumber() == constants::Values::kPracticeTeamNumber;
+
set_max_drivetrain_voltage(12.0);
replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
event_loop->OnRun([this, event_loop]() {
@@ -99,30 +98,32 @@
&auto_splines_, std::placeholders::_1),
SplineDirection::kForward);
starting_position_ = test_spline_->starting_position();
- } else if (FLAGS_target_aligned) {
- target_aligned_splines_ = {
- PlanSpline(std::bind(&AutonomousSplines::TargetAligned1, &auto_splines_,
- std::placeholders::_1, alliance_),
- SplineDirection::kForward),
- PlanSpline(std::bind(&AutonomousSplines::TargetAligned2, &auto_splines_,
- std::placeholders::_1, alliance_),
- SplineDirection::kBackward),
- PlanSpline(std::bind(&AutonomousSplines::TargetAligned3, &auto_splines_,
- std::placeholders::_1, alliance_),
- SplineDirection::kForward)};
- starting_position_ = target_aligned_splines_.value()[0].starting_position();
- CHECK(starting_position_);
- } else if (FLAGS_target_offset) {
- target_offset_splines_ = {
- PlanSpline(std::bind(&AutonomousSplines::TargetOffset1, &auto_splines_,
- std::placeholders::_1),
- SplineDirection::kForward),
- PlanSpline(std::bind(&AutonomousSplines::TargetOffset2, &auto_splines_,
- std::placeholders::_1),
- SplineDirection::kBackward)};
- starting_position_ = target_offset_splines_.value()[0].starting_position();
} else {
- starting_position_ = Eigen::Vector3d::Zero();
+ if (practice_robot_) {
+ fender_splines_ = {PlanSpline(
+ std::bind(&AutonomousSplines::FarSideFender, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward)};
+ starting_position_ = fender_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
+ } else {
+ target_aligned_splines_ = {
+ PlanSpline(
+ std::bind(&AutonomousSplines::TargetAligned1, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::TargetAligned2, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(
+ std::bind(&AutonomousSplines::TargetAligned3, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward)};
+ starting_position_ =
+ target_aligned_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
+ }
}
is_planned_ = true;
@@ -162,15 +163,14 @@
}
if (FLAGS_spline_auto) {
SplineAuto();
- } else if (FLAGS_target_aligned) {
- TargetAligned();
- } else if (FLAGS_target_offset) {
- TargetOffset();
- } else if (FLAGS_just_shoot) {
- JustShoot();
} else {
- return DriveFwd();
+ if (practice_robot_) {
+ Fender();
+ } else {
+ TargetAligned();
+ }
}
+
return true;
}
@@ -265,6 +265,33 @@
.count();
}
+void AutonomousActor::Fender() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+ CHECK(fender_splines_);
+ auto &splines = *fender_splines_;
+
+ // Spin up.
+ set_shooting(false);
+ set_preloading(true);
+ set_shooter_tracking(false);
+ SendSuperstructureGoal();
+
+ if (!splines[0].WaitForPlan()) return;
+ splines[0].Start();
+
+ if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+
+ SendSuperstructureGoal();
+ std::this_thread::sleep_for(chrono::milliseconds(500));
+ ApplyThrottle(0.2);
+ set_shooting(true);
+ SendSuperstructureGoal();
+ LOG(INFO) << "Took "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count();
+}
+
void AutonomousActor::JustShoot() {
// shoot pre-loaded balls
set_shooter_tracking(true);
@@ -355,11 +382,17 @@
intake_offset = intake_builder.Finish();
}
+ flatbuffers::Offset<superstructure::ShooterGoal> shooter_offset =
+ superstructure::CreateShooterGoal(*builder.fbb(), 400.0, 200.0);
+
superstructure::Goal::Builder superstructure_builder =
builder.MakeBuilder<superstructure::Goal>();
superstructure_builder.add_intake(intake_offset);
superstructure_builder.add_intake_preloading(preloading_);
+ if (!shooter_tracking_ && shooting_) {
+ superstructure_builder.add_shooter(shooter_offset);
+ }
superstructure_builder.add_roller_voltage(roller_voltage_);
superstructure_builder.add_roller_speed_compensation(
kRollerSpeedCompensation);
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 1877d58..abf9ea0 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -49,6 +49,7 @@
void ExtendIntake();
void RetractIntake();
void SplineAuto();
+ void Fender();
void SendStartingPosition(const Eigen::Vector3d &start);
void TargetAligned();
void TargetOffset();
@@ -62,6 +63,8 @@
void Replan();
+ bool practice_robot_ = false;
+
double intake_goal_ = 0.0;
double roller_voltage_ = 0.0;
bool shooting_ = false;
@@ -85,6 +88,8 @@
// Max number of splines is 5
std::optional<std::array<SplineHandle, 3>> target_aligned_splines_;
+ std::optional<std::array<SplineHandle, 1>> fender_splines_;
+
std::optional<SplineHandle> barrel_spline_;
std::optional<SplineHandle> slalom_spline_;
std::optional<SplineHandle> test_spline_;
diff --git a/y2020/actors/splines/9971_to_fender.json b/y2020/actors/splines/9971_to_fender.json
new file mode 100644
index 0000000..3481f9d
--- /dev/null
+++ b/y2020/actors/splines/9971_to_fender.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [12.5006132329368, 15.51123630601629, 16.379093330114006, 14.017538081600794, 14.180421562227894, 15.536083464584115], "spline_y": [1.9185982114880447, 1.9182820428405384, 1.5882896426835666, 5.210732299124419, 5.779290830601553, 5.779456064593902], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 2.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 6.0}]}