Updated 2022 Autonomous
Change-Id: I68dcef53387c909a6014b6ee7d46c2b934695beb
Signed-off-by: Vinay Siva <100024232@mvla.net>
Signed-off-by: Griffin Bui <griffinbui+gerrit@gmail.com>
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/y2022/BUILD b/y2022/BUILD
index 1623ec9..488026e 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -14,6 +14,7 @@
"@ctre_phoenix_cci_athena//:shared_libraries",
],
dirs = [
+ "//y2022/actors:splines",
"//y2022/www:www_files",
],
start_binaries = [
diff --git a/y2022/actors/BUILD b/y2022/actors/BUILD
index 57b1222..3c8c6b8 100644
--- a/y2022/actors/BUILD
+++ b/y2022/actors/BUILD
@@ -1,3 +1,5 @@
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
filegroup(
name = "binaries.stripped",
srcs = [
@@ -14,6 +16,24 @@
visibility = ["//visibility:public"],
)
+filegroup(
+ name = "spline_jsons",
+ srcs = glob([
+ "splines/*.json",
+ ]),
+ visibility = ["//visibility:public"],
+)
+
+aos_downloader_dir(
+ name = "splines",
+ srcs = [
+ ":spline_jsons",
+ ],
+ dir = "splines",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
cc_library(
name = "autonomous_action_lib",
srcs = [
@@ -33,6 +53,7 @@
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
"//frc971/control_loops/drivetrain:localizer_fbs",
+ "//y2022:constants",
"//y2022/control_loops/drivetrain:drivetrain_base",
"//y2022/control_loops/superstructure:superstructure_goal_fbs",
"//y2022/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2022/actors/auto_splines.cc b/y2022/actors/auto_splines.cc
index ce206c8..0c98fed 100644
--- a/y2022/actors/auto_splines.cc
+++ b/y2022/actors/auto_splines.cc
@@ -19,84 +19,35 @@
}
}
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
- flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
- flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
- flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+flatbuffers::Offset<frc971::MultiSpline> FixSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ flatbuffers::Offset<frc971::MultiSpline> spline_offset,
+ aos::Alliance alliance) {
+ frc971::MultiSpline *spline =
+ GetMutableTemporaryPointer(*builder->fbb(), spline_offset);
+ flatbuffers::Vector<float> *spline_x = spline->mutable_spline_x();
+ flatbuffers::Vector<float> *spline_y = spline->mutable_spline_y();
- {
- frc971::Constraint::Builder longitudinal_constraint_builder =
- builder->MakeBuilder<frc971::Constraint>();
- longitudinal_constraint_builder.add_constraint_type(
- frc971::ConstraintType::LONGITUDINAL_ACCELERATION);
- longitudinal_constraint_builder.add_value(1.0);
- longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ if (alliance == aos::Alliance::kBlue) {
+ for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+ spline_x->Mutate(ii, -spline_x->Get(ii));
+ }
+ for (size_t ii = 0; ii < spline_y->size(); ++ii) {
+ spline_y->Mutate(ii, -spline_y->Get(ii));
+ }
}
-
- {
- frc971::Constraint::Builder lateral_constraint_builder =
- builder->MakeBuilder<frc971::Constraint>();
- lateral_constraint_builder.add_constraint_type(
- frc971::ConstraintType::LATERAL_ACCELERATION);
- lateral_constraint_builder.add_value(1.0);
- lateral_constraint_offset = lateral_constraint_builder.Finish();
- }
-
- {
- frc971::Constraint::Builder voltage_constraint_builder =
- builder->MakeBuilder<frc971::Constraint>();
- voltage_constraint_builder.add_constraint_type(
- frc971::ConstraintType::VOLTAGE);
- voltage_constraint_builder.add_value(6.0);
- voltage_constraint_offset = voltage_constraint_builder.Finish();
- }
-
- flatbuffers::Offset<
- flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
- constraints_offset =
- builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
- {longitudinal_constraint_offset, lateral_constraint_offset,
- voltage_constraint_offset});
-
- const float startx = 0.4;
- const float starty = 3.4;
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
- builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
- 0.6f + startx, 0.4f + startx,
- 0.4f + startx, 1.0f + startx});
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
- builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
- starty - 0.3f, starty - 0.7f,
- starty - 1.0f, starty - 1.0f});
-
- frc971::MultiSpline::Builder multispline_builder =
- builder->MakeBuilder<frc971::MultiSpline>();
-
- multispline_builder.add_spline_count(1);
- multispline_builder.add_constraints(constraints_offset);
- multispline_builder.add_spline_x(spline_x_offset);
- multispline_builder.add_spline_y(spline_y_offset);
-
- return multispline_builder.Finish();
+ return spline_offset;
}
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
- builder->fbb()->CreateVector<float>(
- {-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
- builder->fbb()->CreateVector<float>({1.25, 1.25, 1.25, 1.25, 1.25, 1.25});
-
- frc971::MultiSpline::Builder multispline_builder =
- builder->MakeBuilder<frc971::MultiSpline>();
-
- multispline_builder.add_spline_count(1);
- multispline_builder.add_spline_x(spline_x_offset);
- multispline_builder.add_spline_y(spline_y_offset);
-
- return multispline_builder.Finish();
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_, builder->fbb()),
+ alliance);
}
} // namespace actors
diff --git a/y2022/actors/auto_splines.h b/y2022/actors/auto_splines.h
index 70c3569..9da6bc6 100644
--- a/y2022/actors/auto_splines.h
+++ b/y2022/actors/auto_splines.h
@@ -2,8 +2,10 @@
#define Y2022_ACTORS_AUTO_SPLINES_H_
#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/input/joystick_state_generated.h"
/*
The cooridinate system for the autonomous splines is the same as the spline
@@ -16,10 +18,22 @@
class AutonomousSplines {
public:
+ AutonomousSplines()
+ : test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/test_spline.json")) {}
+
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+
+ flatbuffers::Offset<frc971::MultiSpline> TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
+ private:
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
};
} // namespace actors
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index d96dd03..ed1a9ee 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -5,33 +5,253 @@
#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 "y2022/actors/auto_splines.h"
+#include "y2022/constants.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
+
namespace y2022 {
namespace actors {
+namespace {
+constexpr double kExtendIntakeGoal = 0.0;
+constexpr double kRetractIntakeGoal = 1.47;
+constexpr double kRollerVoltage = 12.0;
+constexpr double kCatapultReturnPosition = -0.908;
+} // namespace
using ::aos::monotonic_clock;
+using frc971::CreateProfileParameters;
using ::frc971::ProfileParametersT;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::control_loops::drivetrain::LocalizerControl;
+
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
: frc971::autonomous::BaseAutonomousActor(
- event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+ localizer_control_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::drivetrain::LocalizerControl>(
+ "/drivetrain")),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<control_loops::superstructure::Goal>(
+ "/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<control_loops::superstructure::Status>(
+ "/superstructure")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
+ auto_splines_() {
+ set_max_drivetrain_voltage(12.0);
+ replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
+ event_loop->OnRun([this, event_loop]() {
+ replan_timer_->Setup(event_loop->monotonic_now());
+ button_poll_->Setup(event_loop->monotonic_now(), chrono::milliseconds(50));
+ });
+
+ button_poll_ = event_loop->AddTimer([this]() {
+ const aos::monotonic_clock::time_point now =
+ this->event_loop()->context().monotonic_event_time;
+ if (robot_state_fetcher_.Fetch()) {
+ if (robot_state_fetcher_->user_button()) {
+ user_indicated_safe_to_reset_ = true;
+ MaybeSendStartingPosition();
+ }
+ }
+ if (joystick_state_fetcher_.Fetch()) {
+ if (joystick_state_fetcher_->has_alliance() &&
+ (joystick_state_fetcher_->alliance() != alliance_)) {
+ alliance_ = joystick_state_fetcher_->alliance();
+ is_planned_ = false;
+ // Only kick the planning out by 2 seconds. If we end up enabled in that
+ // second, then we will kick it out further based on the code below.
+ replan_timer_->Setup(now + std::chrono::seconds(2));
+ }
+ if (joystick_state_fetcher_->enabled()) {
+ if (!is_planned_) {
+ // Only replan once we've been disabled for 5 seconds.
+ replan_timer_->Setup(now + std::chrono::seconds(5));
+ }
+ }
+ }
+ });
+}
+
+void AutonomousActor::Replan() {
+ LOG(INFO) << "Alliance " << static_cast<int>(alliance_);
+ if (alliance_ == aos::Alliance::kInvalid) {
+ return;
+ }
+ sent_starting_position_ = false;
+ if (FLAGS_spline_auto) {
+ test_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward);
+
+ starting_position_ = test_spline_->starting_position();
+ }
+
+ is_planned_ = true;
+
+ MaybeSendStartingPosition();
+}
+
+void AutonomousActor::MaybeSendStartingPosition() {
+ if (is_planned_ && user_indicated_safe_to_reset_ &&
+ !sent_starting_position_) {
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+}
void AutonomousActor::Reset() {
InitializeEncoders();
ResetDrivetrain();
+ RetractFrontIntake();
+ RetractBackIntake();
+
+ joystick_state_fetcher_.Fetch();
+ CHECK(joystick_state_fetcher_.get() != nullptr)
+ << "Expect at least one JoystickState message before running auto...";
+ alliance_ = joystick_state_fetcher_->alliance();
}
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams *params) {
Reset();
+ if (!user_indicated_safe_to_reset_) {
+ AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+ // Clear this so that we don't accidentally resend things as soon as we replan
+ // later.
+ user_indicated_safe_to_reset_ = false;
+ is_planned_ = false;
+ starting_position_.reset();
AOS_LOG(INFO, "Params are %d\n", params->mode());
+ if (alliance_ == aos::Alliance::kInvalid) {
+ AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
+ return false;
+ }
+ if (FLAGS_spline_auto) {
+ SplineAuto();
+ }
+
return true;
}
+void AutonomousActor::SendStartingPosition(const Eigen::Vector3d &start) {
+ // Set up the starting position for the blue alliance.
+
+ // TODO(james): Resetting the localizer breaks the left/right statespace
+ // controller. That is a bug, but we can fix that later by not resetting.
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(start(0));
+ localizer_control_builder.add_y(start(1));
+ localizer_control_builder.add_theta(start(2));
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ LOG(INFO) << "User button pressed, x: " << start(0) << " y: " << start(1)
+ << " theta: " << start(2);
+ if (builder.Send(localizer_control_builder.Finish()) !=
+ aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
+ }
+}
+
+void AutonomousActor::SplineAuto() {
+ CHECK(test_spline_);
+
+ if (!test_spline_->WaitForPlan()) return;
+ test_spline_->Start();
+
+ if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::SendSuperstructureGoal() {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_front_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), intake_front_goal_,
+ CreateProfileParameters(*builder.fbb(), 20.0, 60.0));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_back_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), intake_back_goal_,
+ CreateProfileParameters(*builder.fbb(), 20.0, 60.0));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ catapult_return_position_offset =
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), kCatapultReturnPosition,
+ CreateProfileParameters(*builder.fbb(), 9.0, 50.0));
+
+ superstructure::CatapultGoal::Builder catapult_goal_builder(*builder.fbb());
+ catapult_goal_builder.add_shot_position(0.03);
+ catapult_goal_builder.add_shot_velocity(18.0);
+ catapult_goal_builder.add_return_position(catapult_return_position_offset);
+ flatbuffers::Offset<superstructure::CatapultGoal> catapult_goal_offset =
+ catapult_goal_builder.Finish();
+
+ superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_builder.add_intake_front(intake_front_offset);
+ superstructure_builder.add_intake_back(intake_back_offset);
+ superstructure_builder.add_roller_speed_compensation(1.5);
+ superstructure_builder.add_roller_speed_front(roller_front_voltage_);
+ superstructure_builder.add_roller_speed_back(roller_back_voltage_);
+ superstructure_builder.add_transfer_roller_speed(transfer_roller_voltage_);
+ superstructure_builder.add_catapult(catapult_goal_offset);
+ superstructure_builder.add_fire(fire_);
+ superstructure_builder.add_auto_aim(true);
+
+ if (builder.Send(superstructure_builder.Finish()) !=
+ aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+}
+
+void AutonomousActor::ExtendFrontIntake() {
+ set_intake_front_goal(kExtendIntakeGoal);
+ set_roller_front_voltage(kRollerVoltage);
+ set_transfer_roller_voltage(kRollerVoltage);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::RetractFrontIntake() {
+ set_intake_front_goal(kRetractIntakeGoal);
+ set_roller_front_voltage(kRollerVoltage);
+ set_transfer_roller_voltage(0.0);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::ExtendBackIntake() {
+ set_intake_back_goal(kExtendIntakeGoal);
+ set_roller_back_voltage(kRollerVoltage);
+ set_transfer_roller_voltage(-kRollerVoltage);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::RetractBackIntake() {
+ set_intake_back_goal(kRetractIntakeGoal);
+ set_roller_back_voltage(kRollerVoltage);
+ set_transfer_roller_voltage(0.0);
+ SendSuperstructureGoal();
+}
+
} // namespace actors
} // namespace y2022
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index ca13d38..f01da02 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -6,10 +6,18 @@
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2022/actors/auto_splines.h"
+#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
namespace y2022 {
namespace actors {
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+namespace superstructure = y2022::control_loops::superstructure;
+
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
explicit AutonomousActor(::aos::EventLoop *event_loop);
@@ -19,6 +27,66 @@
private:
void Reset();
+
+ void set_intake_front_goal(double intake_front_goal) {
+ intake_front_goal_ = intake_front_goal;
+ }
+ void set_intake_back_goal(double intake_back_goal) {
+ intake_back_goal_ = intake_back_goal;
+ }
+ void set_roller_front_voltage(double roller_front_voltage) {
+ roller_front_voltage_ = roller_front_voltage;
+ }
+ void set_roller_back_voltage(double roller_back_voltage) {
+ roller_back_voltage_ = roller_back_voltage;
+ }
+ void set_transfer_roller_voltage(double voltage) {
+ transfer_roller_voltage_ = voltage;
+ }
+
+ void set_fire_at_will(bool fire) { fire_ = fire; }
+
+ void SendSuperstructureGoal();
+ void ExtendFrontIntake();
+ void RetractFrontIntake();
+ void ExtendBackIntake();
+ void RetractBackIntake();
+ void SendStartingPosition(const Eigen::Vector3d &start);
+ void MaybeSendStartingPosition();
+
+ void SplineAuto();
+
+ void Replan();
+
+ double intake_front_goal_ = 0.0;
+ double intake_back_goal_ = 0.0;
+ double roller_front_voltage_ = 0.0;
+ double roller_back_voltage_ = 0.0;
+ double transfer_roller_voltage_ = 0.0;
+ bool fire_ = false;
+
+ aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
+ localizer_control_sender_;
+ aos::Sender<y2022::control_loops::superstructure::Goal>
+ superstructure_goal_sender_;
+ aos::Fetcher<y2022::control_loops::superstructure::Status>
+ superstructure_status_fetcher_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ aos::Fetcher<aos::RobotState> robot_state_fetcher_;
+
+ aos::TimerHandler *replan_timer_;
+ aos::TimerHandler *button_poll_;
+
+ std::optional<SplineHandle> test_spline_;
+
+ aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ AutonomousSplines auto_splines_;
+ bool user_indicated_safe_to_reset_ = false;
+ bool sent_starting_position_ = false;
+
+ bool is_planned_ = false;
+
+ std::optional<Eigen::Vector3d> starting_position_;
};
} // namespace actors
diff --git a/y2022/actors/splines/README.md b/y2022/actors/splines/README.md
new file mode 100644
index 0000000..c655416
--- /dev/null
+++ b/y2022/actors/splines/README.md
@@ -0,0 +1,3 @@
+# Spline Descriptions
+This folder contains reference material for what each spline does
+
diff --git a/y2022/actors/splines/test_spline.json b/y2022/actors/splines/test_spline.json
new file mode 100644
index 0000000..733d516
--- /dev/null
+++ b/y2022/actors/splines/test_spline.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}