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 =