commit 546a0389bfe4e0db646c39d55e8352555f9b77c7
author Austin Schuh <austin.linux@gmail.com> Sun Apr 16 19:10:18 2017 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun May 28 17:08:21 2017 -0700
tree 7495f45b37c6a5cd6be08d85f65b0dee62e2abc7
parent d85c66e12cae1c5b0738b841896cb0a45fb273ad

Adjusted indexer for the lower gearing.

Change-Id: Ia163de29c8c247f0490bdd26ff2f9ddf2c66bf09

frc971/autonomous/base_autonomous_actor.cc

diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index d115246..5311810 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -266,6 +266,54 @@
   }
 }
 
+bool BaseAutonomousActor::WaitForTurnProfileDone() {
+  ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+                                      ::std::chrono::milliseconds(5) / 2);
+  constexpr double kProfileTolerance = 0.001;
+
+  while (true) {
+    if (ShouldCancel()) {
+      return false;
+    }
+    phased_loop.SleepUntilNext();
+    drivetrain_queue.status.FetchLatest();
+    if (drivetrain_queue.status.get()) {
+      const double left_profile_error =
+          (initial_drivetrain_.left -
+           drivetrain_queue.status->profiled_left_position_goal);
+      const double right_profile_error =
+          (initial_drivetrain_.right -
+           drivetrain_queue.status->profiled_right_position_goal);
+
+      const double profile_angle_to_go =
+          (right_profile_error - left_profile_error) /
+          (dt_config_.robot_radius * 2.0);
+
+      if (::std::abs(profile_angle_to_go) < kProfileTolerance) {
+        LOG(INFO, "Finished turn profile\n");
+        return true;
+      }
+    }
+  }
+}
+
+double BaseAutonomousActor::DriveDistanceLeft() {
+  using ::frc971::control_loops::drivetrain_queue;
+  drivetrain_queue.status.FetchLatest();
+  if (drivetrain_queue.status.get()) {
+    const double left_error =
+        (initial_drivetrain_.left -
+         drivetrain_queue.status->estimated_left_position);
+    const double right_error =
+        (initial_drivetrain_.right -
+         drivetrain_queue.status->estimated_right_position);
+
+    return (left_error + right_error) / 2.0;
+  } else {
+    return 0;
+  }
+}
+
 ::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
     const AutonomousActionParams &params) {
   return ::std::unique_ptr<AutonomousAction>(