commit 9c326f5a2d89416f337da847d9e1e168ff59c48c
author Ravago Jones <ravagojones@gmail.com> Sat Mar 20 15:00:16 2021 -0700
committer Ravago Jones <ravagojones@gmail.com> Sat Mar 20 16:03:22 2021 -0700
tree 84500aed419a4b118f951c6eef8c6a37541870e5
parent aaf3d0029231b86c0149550a40a21f8ec94a4207

Make AutoNav Autonomouses

Change-Id: I7c036cdf16f3b850f4c91dfb5deb0d133d7f0a50

y2020/actors/autonomous_actor.cc

diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 4223332..d4306ac 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -15,6 +15,9 @@
 DEFINE_bool(spline_auto, true, "If true, define a spline autonomous mode");
 DEFINE_bool(galactic_search, false,
             "If true, do the galactic search autonomous");
+DEFINE_bool(bounce, false, "If true, run the AutoNav Bounce autonomous");
+DEFINE_bool(barrel, false, "If true, run the AutoNav Barrel autonomous");
+DEFINE_bool(slalom, false, "If true, run the AutoNav Slalom autonomous");
 
 namespace y2020 {
 namespace actors {
@@ -59,6 +62,12 @@
   }
   if (FLAGS_galactic_search) {
     GalacticSearch();
+  } else if (FLAGS_bounce) {
+    AutoNavBounce();
+  } else if (FLAGS_barrel) {
+    AutoNavBarrel();
+  } else if (FLAGS_slalom) {
+    AutoNavSlalom();
   } else if (FLAGS_spline_auto) {
     SplineAuto();
   } else {
@@ -129,6 +138,93 @@
   }
 }
 
+void AutonomousActor::AutoNavBounce() {
+  SplineHandle spline1 = PlanSpline(
+      [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+                 *builder) {
+        flatbuffers::Offset<frc971::MultiSpline> target_spline =
+            auto_splines_.AutoNavBounce1(builder);
+        const frc971::MultiSpline *const spline =
+            flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
+        SendStartingPosition(CHECK_NOTNULL(spline));
+        return target_spline;
+      },
+      SplineDirection::kForward);
+
+  if (!spline1.WaitForPlan()) return;
+  spline1.Start();
+
+  SplineHandle spline2 = PlanSpline(
+      [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+                 *builder) { return auto_splines_.AutoNavBounce2(builder); },
+      SplineDirection::kBackward);
+
+  if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
+
+  if (!spline2.WaitForPlan()) return;
+  spline2.Start();
+
+  SplineHandle spline3 = PlanSpline(
+      [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+                 *builder) { return auto_splines_.AutoNavBounce3(builder); },
+      SplineDirection::kForward);
+
+  if (!spline2.WaitForSplineDistanceRemaining(0.02)) return;
+
+  if (!spline3.WaitForPlan()) return;
+  spline3.Start();
+
+  SplineHandle spline4 = PlanSpline(
+      [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+                 *builder) { return auto_splines_.AutoNavBounce4(builder); },
+      SplineDirection::kBackward);
+
+  if (!spline3.WaitForSplineDistanceRemaining(0.02)) return;
+
+  if (!spline4.WaitForPlan()) return;
+  spline4.Start();
+
+  if (!spline4.WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::AutoNavBarrel() {
+  SplineHandle spline1 = PlanSpline(
+      [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+                 *builder) {
+        flatbuffers::Offset<frc971::MultiSpline> target_spline;
+        target_spline = auto_splines_.AutoNavBarrel(builder);
+        const frc971::MultiSpline *const spline =
+            flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
+        SendStartingPosition(CHECK_NOTNULL(spline));
+        return target_spline;
+      },
+      SplineDirection::kForward);
+
+  if (!spline1.WaitForPlan()) return;
+  spline1.Start();
+
+  if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::AutoNavSlalom() {
+  SplineHandle spline1 = PlanSpline(
+      [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+                 *builder) {
+        flatbuffers::Offset<frc971::MultiSpline> target_spline;
+        target_spline = auto_splines_.AutoNavSlalom(builder);
+        const frc971::MultiSpline *const spline =
+            flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
+        SendStartingPosition(CHECK_NOTNULL(spline));
+        return target_spline;
+      },
+      SplineDirection::kForward);
+
+  if (!spline1.WaitForPlan()) return;
+  spline1.Start();
+
+  if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
+}
+
 void AutonomousActor::SplineAuto() {
   SplineHandle spline1 = PlanSpline(
       [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder