commit a3c148e9880a6ea2bc64649b534022a03813c2f3
author Austin Schuh <austin.linux@gmail.com> Fri Mar 09 21:04:05 2018 -0800
committer Austin Schuh <austin.linux@gmail.com> Sun Mar 11 01:12:51 2018 -0800
tree 0f8b0ab7903bb0c640488dea491df97b42f6cb9b
parent 876b4f0cea61475504d5399519729ca58809d8e8

Basic drive in a square auto mode.

Change-Id: Idbe7b8c7b3738ff12451ff80209874cf4c47a147

y2018/actors/autonomous_actor.cc

diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
new file mode 100644
index 0000000..83a1295
--- /dev/null
+++ b/y2018/actors/autonomous_actor.cc
@@ -0,0 +1,69 @@
+#include "y2018/actors/autonomous_actor.h"
+
+#include <inttypes.h>
+
+#include <chrono>
+#include <cmath>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2018/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2018 {
+namespace actors {
+using ::frc971::ProfileParameters;
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+namespace this_thread = ::std::this_thread;
+
+namespace {
+
+double DoubleSeconds(monotonic_clock::duration duration) {
+  return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
+      .count();
+}
+
+const ProfileParameters kDrive = {1.5, 2.0};
+const ProfileParameters kTurn = {3.0, 3.0};
+
+}  // namespace
+
+AutonomousActor::AutonomousActor(
+    ::frc971::autonomous::AutonomousActionQueueGroup *s)
+    : frc971::autonomous::BaseAutonomousActor(
+          s, control_loops::drivetrain::GetDrivetrainConfig()) {}
+
+bool AutonomousActor::RunAction(
+    const ::frc971::autonomous::AutonomousActionParams &params) {
+  monotonic_clock::time_point start_time = monotonic_clock::now();
+  LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
+  Reset();
+
+  for (int i = 0; i < 4; ++i) {
+    StartDrive(0.0, M_PI / 2.0, kDrive, kTurn);
+    if (!WaitForTurnProfileDone()) return true;
+
+    // Drive, but get within 0.3 meters
+    StartDrive(1.0, 0.0, kDrive, kTurn);
+    if (!WaitForDriveProfileDone()) return true;
+  }
+
+  LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+
+  ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+                                      ::std::chrono::milliseconds(5) / 2);
+
+  while (!ShouldCancel()) {
+    phased_loop.SleepUntilNext();
+  }
+  LOG(DEBUG, "Done running\n");
+
+  return true;
+}
+
+}  // namespace actors
+}  // namespace y2018