commit 6bcc2305184706f2644992385342220ebf35becc
author Austin Schuh <austin.linux@gmail.com> Sat Mar 23 22:28:06 2019 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Mar 24 14:36:28 2019 -0700
tree 68986b55474c8a771aa2e57b4a058fd7e6e760a2
parent cb94024740a4f617603a5e5553017c7c3cc7d2d3

Add spline management to base_autonomous_actor

We can now start and run splines.  And it works!

Change-Id: Ic58cf8fb298538bd5446f00c2989f817561f9db1

frc971/autonomous/base_autonomous_actor.cc

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 &params) {
   return ::std::unique_ptr<AutonomousAction>(

frc971/autonomous/base_autonomous_actor.h

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 =