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.h

diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
new file mode 100644
index 0000000..b8d58e2
--- /dev/null
+++ b/y2018/actors/autonomous_actor.h
@@ -0,0 +1,85 @@
+#ifndef Y2018_ACTORS_AUTONOMOUS_ACTOR_H_
+#define Y2018_ACTORS_AUTONOMOUS_ACTOR_H_
+
+#include <chrono>
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "y2018/control_loops/superstructure/arm/generated_graph.h"
+#include "y2018/control_loops/superstructure/superstructure.q.h"
+
+namespace y2018 {
+namespace actors {
+using ::y2018::control_loops::superstructure_queue;
+
+namespace arm = ::y2018::control_loops::superstructure::arm;
+
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+ public:
+  explicit AutonomousActor(::frc971::autonomous::AutonomousActionQueueGroup *s);
+
+  bool RunAction(
+      const ::frc971::autonomous::AutonomousActionParams &params) override;
+ private:
+  void Reset() {
+    roller_voltage_ = 0.0;
+    left_intake_angle_ = -3.3;
+    right_intake_angle_ = -3.3;
+    arm_goal_position_ = arm::NeutralIndex();
+    grab_box_ = false;
+    open_claw_ = false;
+    deploy_fork_ = false;
+    InitializeEncoders();
+    ResetDrivetrain();
+    SendSuperstructureGoal();
+  }
+
+  double roller_voltage_ = 0.0;
+  double left_intake_angle_ = -3.3;
+  double right_intake_angle_ = -3.3;
+  uint32_t arm_goal_position_ = arm::NeutralIndex();
+  bool grab_box_ = false;
+  bool open_claw_ = false;
+  bool deploy_fork_ = false;
+
+  void set_roller_voltage(double roller_voltage) {
+    roller_voltage_ = roller_voltage;
+  }
+  void set_left_intake_angle(double left_intake_angle) {
+    left_intake_angle_ = left_intake_angle;
+  }
+  void set_right_intake_angle(double right_intake_angle) {
+    right_intake_angle_ = right_intake_angle;
+  }
+  void set_arm_goal_position(uint32_t arm_goal_position) {
+    arm_goal_position_ = arm_goal_position;
+  }
+  void set_grab_box(bool grab_box) { grab_box_ = grab_box; }
+  void set_open_claw(bool open_claw) { open_claw_ = open_claw; }
+  void set_deploy_fork(bool deploy_fork) { deploy_fork_ = deploy_fork; }
+
+  void SendSuperstructureGoal() {
+    auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+    new_superstructure_goal->intake.roller_voltage = roller_voltage_;
+    new_superstructure_goal->intake.left_intake_angle = left_intake_angle_;
+    new_superstructure_goal->intake.right_intake_angle = right_intake_angle_;
+
+    new_superstructure_goal->arm_goal_position = arm_goal_position_;
+    new_superstructure_goal->grab_box = grab_box_;
+    new_superstructure_goal->open_claw = open_claw_;
+    new_superstructure_goal->deploy_fork = deploy_fork_;
+
+    if (!new_superstructure_goal.Send()) {
+      LOG(ERROR, "Sending superstructure goal failed.\n");
+    }
+  }
+};
+
+}  // namespace actors
+}  // namespace y2018
+
+#endif  // Y2018_ACTORS_AUTONOMOUS_ACTOR_H_