commit 4bd29b17bf604b83805f0f16ea866680eb9e8801
author Philipp Schrader <philipp.schrader@gmail.com> Wed Feb 22 04:42:27 2017 +0000
committer Philipp Schrader <philipp.schrader@gmail.com> Wed Feb 22 04:43:46 2017 +0000
tree 828226f1ad9494f6a425fc200296785c2cec8f66
parent 0fc1e6dd63fcaea7dc2298fce0fcb74a8459f80e

Factor out drivetrain functionality from y2016 auton actor

I hope that we can re-use this in the y2017 auton actor.

Change-Id: I1258a5ef99a706ebefaba8e2238bb80df68d586f

frc971/autonomous/base_autonomous_actor.h

diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
new file mode 100644
index 0000000..69e2e5a
--- /dev/null
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -0,0 +1,69 @@
+#ifndef FRC971_AUTONOMOUS_BASE_AUTONOMOUS_ACTOR_H_
+#define FRC971_AUTONOMOUS_BASE_AUTONOMOUS_ACTOR_H_
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace frc971 {
+namespace autonomous {
+
+class BaseAutonomousActor
+    : public ::aos::common::actions::ActorBase<AutonomousActionQueueGroup> {
+ public:
+  explicit BaseAutonomousActor(
+      AutonomousActionQueueGroup *s,
+      const control_loops::drivetrain::DrivetrainConfig dt_config);
+
+ protected:
+  void ResetDrivetrain();
+  void InitializeEncoders();
+  void StartDrive(double distance, double angle, ProfileParameters linear,
+                  ProfileParameters angular);
+
+  void WaitUntilDoneOrCanceled(
+      ::std::unique_ptr<aos::common::actions::Action> action);
+  // Waits for the drive motion to finish.  Returns true if it succeeded, and
+  // false if it cancels.
+  bool WaitForDriveDone();
+
+  // Returns true if the drive has finished.
+  bool IsDriveDone();
+
+  // Waits until the robot is pitched up above the specified angle, or the move
+  // finishes.  Returns true on success, and false if it cancels.
+  bool WaitForAboveAngle(double angle);
+  bool WaitForBelowAngle(double angle);
+  bool WaitForMaxBy(double angle);
+
+  // Waits until the profile and distance is within distance and angle of the
+  // goal.  Returns true on success, and false when canceled.
+  bool WaitForDriveNear(double distance, double angle);
+
+  bool WaitForDriveProfileDone();
+
+  const control_loops::drivetrain::DrivetrainConfig dt_config_;
+
+  // Initial drivetrain positions.
+  struct InitialDrivetrain {
+    double left;
+    double right;
+  };
+  InitialDrivetrain initial_drivetrain_;
+};
+
+using AutonomousAction =
+    ::aos::common::actions::TypedAction<AutonomousActionQueueGroup>;
+
+// Makes a new AutonomousActor action.
+::std::unique_ptr<AutonomousAction> MakeAutonomousAction(
+    const AutonomousActionParams &params);
+
+}  // namespace autonomous
+}  // namespace frc971
+
+#endif  // FRC971_AUTONOMOUS_BASE_AUTONOMOUS_ACTOR_H_