Add alliance-switching to 2020 auto
Add code to automatically switch the side of the field we initialize to
based on the alliance reported by the driver's station (this can be set
from the driver's station when not connected to the FMS).
Change-Id: I8c7576e451a0f0762b29f2fe8c407791900c4727
diff --git a/y2020/actors/BUILD b/y2020/actors/BUILD
index 668c502..b294a51 100644
--- a/y2020/actors/BUILD
+++ b/y2020/actors/BUILD
@@ -27,6 +27,8 @@
deps = [
"//aos/events:event_loop",
"//aos/logging",
+ "//aos/robot_state:joystick_state_fbs",
+ "//aos/util:math",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops:control_loops_fbs",
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 3498a3e..b6d97c4 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -20,7 +20,8 @@
}
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Alliance alliance) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
@@ -30,7 +31,7 @@
builder->MakeBuilder<frc971::Constraint>();
longitudinal_constraint_builder.add_constraint_type(
frc971::ConstraintType::LONGITUDINAL_ACCELERATION);
- longitudinal_constraint_builder.add_value(1.0);
+ longitudinal_constraint_builder.add_value(2.0);
longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
}
@@ -39,7 +40,7 @@
builder->MakeBuilder<frc971::Constraint>();
lateral_constraint_builder.add_constraint_type(
frc971::ConstraintType::LATERAL_ACCELERATION);
- lateral_constraint_builder.add_value(1.0);
+ lateral_constraint_builder.add_value(2.0);
lateral_constraint_offset = lateral_constraint_builder.Finish();
}
@@ -48,7 +49,7 @@
builder->MakeBuilder<frc971::Constraint>();
voltage_constraint_builder.add_constraint_type(
frc971::ConstraintType::VOLTAGE);
- voltage_constraint_builder.add_value(6.0);
+ voltage_constraint_builder.add_value(8.0);
voltage_constraint_offset = voltage_constraint_builder.Finish();
}
@@ -59,16 +60,22 @@
{longitudinal_constraint_offset, lateral_constraint_offset,
voltage_constraint_offset});
- const float startx = 0.0;
- const float starty = 0.05;
+ const float startx = 3.3;
+ const float starty = -.7;
+ std::vector<float> x_pos{0.0f + startx, 0.8f + startx, 0.8f + startx,
+ 1.2f + startx, 1.2f + startx, 2.0f + startx};
+ std::vector<float> y_pos{starty - 0.0f, starty - 0.0f, starty - 0.1f,
+ starty - 0.2f, starty - 0.3f, starty - 0.3f};
+ if (alliance == aos::Alliance::kRed) {
+ for (size_t ii = 0; ii < x_pos.size(); ++ii) {
+ x_pos[ii] *= -1;
+ y_pos[ii] *= -1;
+ }
+ }
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
- builder->fbb()->CreateVector<float>({0.0f + startx, 0.8f + startx,
- 0.8f + startx, 1.2f + startx,
- 1.2f + startx, 2.0f + startx});
+ builder->fbb()->CreateVector<float>(x_pos);
flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
- builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
- starty - 0.1f, starty - 0.2f,
- starty - 0.3f, starty - 0.3f});
+ builder->fbb()->CreateVector<float>(y_pos);
frc971::MultiSpline::Builder multispline_builder =
builder->MakeBuilder<frc971::MultiSpline>();
diff --git a/y2020/actors/auto_splines.h b/y2020/actors/auto_splines.h
index 471b7b9..96f055f 100644
--- a/y2020/actors/auto_splines.h
+++ b/y2020/actors/auto_splines.h
@@ -2,6 +2,7 @@
#define y2020_ACTORS_AUTO_SPLINES_H_
#include "aos/events/event_loop.h"
+#include "aos/robot_state/joystick_state_generated.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
/*
@@ -17,7 +18,8 @@
class AutonomousSplines {
public:
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ aos::Alliance alliance);
static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
};
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index e87e064..0b2b9c6 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -6,6 +6,7 @@
#include <cmath>
#include "aos/logging/logging.h"
+#include "aos/util/math.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/actors/auto_splines.h"
@@ -23,21 +24,36 @@
event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
localizer_control_sender_(event_loop->MakeSender<
::frc971::control_loops::drivetrain::LocalizerControl>(
- "/drivetrain")) {}
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {}
void AutonomousActor::Reset() {
InitializeEncoders();
ResetDrivetrain();
+ joystick_state_fetcher_.Fetch();
+ CHECK(joystick_state_fetcher_.get() != nullptr)
+ << "Expect at least one JoystickState message before running auto...";
+ alliance_ = joystick_state_fetcher_->alliance();
+ // Set up the starting position for the blue alliance.
+ // Currently just arbitrarily chosen for testing purposes, such that the
+ // robot starts on the side of the field nearest where it would score
+ // (although I do not know if this is actually a legal starting position).
+ Eigen::Vector2d starting_position(3.3, -0.7);
+ double starting_heading = 0.0;
+ if (alliance_ == aos::Alliance::kRed) {
+ starting_position *= -1.0;
+ starting_heading = aos::math::NormalizeAngle(starting_heading + M_PI);
+ }
{
auto builder = localizer_control_sender_.MakeBuilder();
LocalizerControl::Builder localizer_control_builder =
builder.MakeBuilder<LocalizerControl>();
- // TODO(james): Set starting position based on the alliance.
- localizer_control_builder.add_x(0.0);
- localizer_control_builder.add_y(0.0);
- localizer_control_builder.add_theta(0.0);
+ localizer_control_builder.add_x(starting_position.x());
+ localizer_control_builder.add_y(starting_position.y());
+ localizer_control_builder.add_theta(starting_heading);
localizer_control_builder.add_theta_uncertainty(0.00001);
if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
@@ -48,9 +64,14 @@
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams *params) {
Reset();
+ if (alliance_ == aos::Alliance::kInvalid) {
+ AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
+ return false;
+ }
- SplineHandle spline1 =
- PlanSpline(AutonomousSplines::BasicSSpline, SplineDirection::kForward);
+ SplineHandle spline1 = PlanSpline(std::bind(AutonomousSplines::BasicSSpline,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward);
if (!spline1.WaitForPlan()) return true;
spline1.Start();
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index d86feab..747fa8d 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -23,6 +23,8 @@
::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ aos::Alliance alliance_ = aos::Alliance::kInvalid;
};
} // namespace actors