Add spline management to base_autonomous_actor
We can now start and run splines. And it works!
Change-Id: Ic58cf8fb298538bd5446f00c2989f817561f9db1
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index ac1b3b7..ade396e 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -66,7 +66,7 @@
drivetrain_message->linear = linear;
drivetrain_message->angular = angular;
- LOG_STRUCT(DEBUG, "drivetrain_goal", *drivetrain_message);
+ LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
drivetrain_message.Send();
}
@@ -349,6 +349,96 @@
}
}
+BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
+ const ::frc971::MultiSpline &spline) {
+ LOG(INFO, "Planning spline\n");
+
+ int32_t spline_handle = (++spline_handle_) | ((getpid() & 0xFFFF) << 15);
+
+ drivetrain_queue.goal.FetchLatest();
+
+ auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
+ drivetrain_message->controller_type = 2;
+
+ drivetrain_message->spline = spline;
+ drivetrain_message->spline.spline_idx = spline_handle;
+ drivetrain_message->spline_handle = goal_spline_handle_;
+
+ LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
+
+ drivetrain_message.Send();
+
+ return BaseAutonomousActor::SplineHandle(spline_handle, this);
+}
+
+bool BaseAutonomousActor::SplineHandle::IsPlanned() {
+ drivetrain_queue.status.FetchLatest();
+ LOG_STRUCT(INFO, "dts", *drivetrain_queue.status.get());
+ if (drivetrain_queue.status.get() &&
+ ((drivetrain_queue.status->trajectory_logging.planning_spline_idx ==
+ spline_handle_ &&
+ drivetrain_queue.status->trajectory_logging.planning_state == 3) ||
+ drivetrain_queue.status->trajectory_logging.current_spline_idx ==
+ spline_handle_)) {
+ return true;
+ }
+ return false;
+}
+
+bool BaseAutonomousActor::SplineHandle::WaitForPlan() {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (base_autonomous_actor_->ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ if (IsPlanned()) {
+ return true;
+ }
+ }
+}
+
+void BaseAutonomousActor::SplineHandle::Start() {
+ auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
+ drivetrain_message->controller_type = 2;
+
+ LOG(INFO, "Starting spline\n");
+
+ drivetrain_message->spline_handle = spline_handle_;
+ base_autonomous_actor_->goal_spline_handle_ = spline_handle_;
+
+ LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
+
+ drivetrain_message.Send();
+}
+
+bool BaseAutonomousActor::SplineHandle::IsDone() {
+ drivetrain_queue.status.FetchLatest();
+ LOG_STRUCT(INFO, "dts", *drivetrain_queue.status.get());
+
+ if (drivetrain_queue.status.get() &&
+ drivetrain_queue.status->trajectory_logging.current_spline_idx ==
+ spline_handle_) {
+ return false;
+ }
+ return true;
+}
+
+bool BaseAutonomousActor::SplineHandle::WaitForDone() {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (base_autonomous_actor_->ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ if (IsDone()) {
+ return true;
+ }
+ }
+}
+
::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
const AutonomousActionParams ¶ms) {
return ::std::unique_ptr<AutonomousAction>(
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index dd53a7d..7e3ef35 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -20,6 +20,32 @@
const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
protected:
+ class SplineHandle {
+ public:
+ bool IsPlanned();
+ bool WaitForPlan();
+ void Start();
+
+ bool IsDone();
+ bool WaitForDone();
+
+ // Wait for done, wait until X from the end, wait for distance from the end
+
+ private:
+ friend BaseAutonomousActor;
+ SplineHandle(int32_t spline_handle,
+ BaseAutonomousActor *base_autonomous_actor)
+ : spline_handle_(spline_handle),
+ base_autonomous_actor_(base_autonomous_actor) {}
+
+ int32_t spline_handle_;
+ BaseAutonomousActor *base_autonomous_actor_;
+ };
+
+ // Starts planning the spline, and returns a handle to be used to manipulate
+ // it.
+ SplineHandle PlanSpline(const ::frc971::MultiSpline &spline);
+
void ResetDrivetrain();
void InitializeEncoders();
void StartDrive(double distance, double angle, ProfileParameters linear,
@@ -67,7 +93,14 @@
}
private:
+ friend class SplineHandle;
+
double max_drivetrain_voltage_ = 12.0;
+
+ // Unique counter so we get unique spline handles.
+ int spline_handle_ = 0;
+ // Last goal spline handle;
+ int32_t goal_spline_handle_ = 0;
};
using AutonomousAction =
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 7f778f7..73ebe10 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -46,8 +46,14 @@
// State of the spline execution.
bool is_executing;
- int32_t current_spline_handle;
+ // The handle of the goal spline. 0 means stop requested.
+ int32_t goal_spline_handle;
+ // Handle of the executing spline. -1 means none requested. If there was no
+ // spline executing when a spline finished optimizing, it will become the
+ // current spline even if we aren't ready to start yet.
int32_t current_spline_idx;
+ // Handle of the spline that is being optimized and staged.
+ int32_t planning_spline_idx;
// Expected position and velocity on the spline
float x;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 2621007..0b6026d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -41,6 +41,7 @@
plan_state_ = PlanState::kBuildingTrajectory;
const ::frc971::MultiSpline &multispline = goal_.spline;
future_spline_idx_ = multispline.spline_idx;
+ planning_spline_idx_ = future_spline_idx_;
auto x = multispline.spline_x;
auto y = multispline.spline_y;
::std::vector<Spline> splines = ::std::vector<Spline>();
@@ -187,8 +188,15 @@
}
status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
status->trajectory_logging.is_executing = !IsAtEnd();
- status->trajectory_logging.current_spline_handle = current_spline_handle_;
+ status->trajectory_logging.goal_spline_handle = current_spline_handle_;
status->trajectory_logging.current_spline_idx = current_spline_idx_;
+
+ int32_t planning_spline_idx = planning_spline_idx_;
+ if (current_spline_idx_ == planning_spline_idx) {
+ status->trajectory_logging.planning_spline_idx = 0;
+ } else {
+ status->trajectory_logging.planning_spline_idx = planning_spline_idx_;
+ }
}
}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index c9ebbe4..c165693 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -99,6 +99,7 @@
::std::unique_ptr<Trajectory> past_trajectory_;
::std::unique_ptr<Trajectory> future_trajectory_;
int32_t future_spline_idx_ = 0; // Current spline being computed.
+ ::std::atomic<int32_t> planning_spline_idx_{-1};
// TODO(alex): pull this out of dt_config.
const ::Eigen::DiagonalMatrix<double, 5> Q =
diff --git a/y2019/actors/BUILD b/y2019/actors/BUILD
index e772718..ee70772 100644
--- a/y2019/actors/BUILD
+++ b/y2019/actors/BUILD
@@ -17,9 +17,11 @@
cc_library(
name = "autonomous_action_lib",
srcs = [
+ "auto_splines.cc",
"autonomous_actor.cc",
],
hdrs = [
+ "auto_splines.h",
"autonomous_actor.h",
],
deps = [
diff --git a/y2019/actors/auto_splines.cc b/y2019/actors/auto_splines.cc
new file mode 100644
index 0000000..f316037
--- /dev/null
+++ b/y2019/actors/auto_splines.cc
@@ -0,0 +1,75 @@
+#include "y2019/actors/auto_splines.h"
+
+#include "frc971/control_loops/control_loops.q.h"
+
+namespace y2019 {
+namespace actors {
+
+::frc971::MultiSpline AutonomousSplines::HPToNearRocket() {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint lateral_constraint;
+ ::frc971::Constraint voltage_constraint;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 1.0;
+
+ lateral_constraint.constraint_type = 2;
+ lateral_constraint.value = 1.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 12.0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{0.4, 1.0, 3.0, 4.0, 4.5, 5.05}};
+ spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.5}};
+ spline.constraints = {
+ {longitudinal_constraint, lateral_constraint, voltage_constraint}};
+ return spline;
+}
+
+::frc971::MultiSpline AutonomousSplines::BasicSSpline() {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint lateral_constraint;
+ ::frc971::Constraint voltage_constraint;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 1.0;
+
+ lateral_constraint.constraint_type = 2;
+ lateral_constraint.value = 1.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 6.0;
+
+ spline.spline_count = 1;
+ const float startx = 0.4;
+ const float starty = 3.4;
+ spline.spline_x = {{0.0f + startx, 0.6f + startx, 0.6f + startx,
+ 0.4f + startx, 0.4f + startx, 1.0f + startx}};
+ spline.spline_y = {{starty - 0.0f, starty - 0.0f, starty - 0.3f,
+ starty - 0.7f, starty - 1.0f, starty - 1.0f}};
+ spline.constraints = {
+ {longitudinal_constraint, lateral_constraint, voltage_constraint}};
+ return spline;
+}
+
+::frc971::MultiSpline AutonomousSplines::StraightLine() {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint contraints;
+
+ contraints.constraint_type = 0;
+ contraints.value = 0.0;
+ contraints.start_distance = 0.0;
+ contraints.end_distance = 0.0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{-12.3, -11.9, -11.5, -11.1, -10.6, -10.0}};
+ spline.spline_y = {{1.25, 1.25, 1.25, 1.25, 1.25, 1.25}};
+ spline.constraints = {{contraints}};
+ return spline;
+}
+
+} // namespace actors
+} // namespace y2019
diff --git a/y2019/actors/auto_splines.h b/y2019/actors/auto_splines.h
new file mode 100644
index 0000000..7e79d1f
--- /dev/null
+++ b/y2019/actors/auto_splines.h
@@ -0,0 +1,30 @@
+#ifndef Y2019_ACTORS_AUTO_SPLINES_H_
+#define Y2019_ACTORS_AUTO_SPLINES_H_
+
+#include "frc971/control_loops/control_loops.q.h"
+/*
+
+ The cooridinate system for the autonomous splines is the same as the spline
+ python generator and drivetrain spline systems.
+
+*/
+
+namespace y2019 {
+namespace actors {
+
+class AutonomousSplines {
+ public:
+ // A spline that does an 's' cause that's what he wanted.
+ static ::frc971::MultiSpline BasicSSpline();
+
+ // Straight
+ static ::frc971::MultiSpline StraightLine();
+
+ // HP to near side rocket
+ static ::frc971::MultiSpline HPToNearRocket();
+};
+
+} // namespace actors
+} // namespace y2019
+
+#endif // Y2019_ACTORS_AUTO_SPLINES_H_
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index c14ef3c..6f16f44 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -10,6 +10,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "y2019/actors/auto_splines.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
namespace y2019 {