commit 624088a369e23a735edfd43e634f30e777d4e851
author Austin Schuh <austin.linux@gmail.com> Wed Mar 22 22:36:16 2017 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 25 15:17:09 2017 -0700
tree 216e083a8fcdf013546238aa91be9d634d1376b3
parent 6a42174efc8b1776297ca893115f25bf7ee79b2d

Added 40 kpa auto

Change-Id: I1985625515b02bac1d909900c9593b9c88b3d333

y2017/actors/autonomous_actor.h

diff --git a/y2017/actors/autonomous_actor.h b/y2017/actors/autonomous_actor.h
index 3c179ed..fcb17e9 100644
--- a/y2017/actors/autonomous_actor.h
+++ b/y2017/actors/autonomous_actor.h
@@ -9,11 +9,14 @@
 #include "frc971/autonomous/base_autonomous_actor.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
 
 namespace y2017 {
 namespace actors {
 using ::frc971::ProfileParameters;
 
+using ::y2017::control_loops::superstructure_queue;
+
 class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
  public:
   explicit AutonomousActor(::frc971::autonomous::AutonomousActionQueueGroup *s);
@@ -21,7 +24,77 @@
   bool RunAction(
       const ::frc971::autonomous::AutonomousActionParams &params) override;
  private:
-  // TODO(phil): Implement this.
+  void Reset() {
+    intake_goal_ = 0.0;
+    turret_goal_ = 0.0;
+    vision_track_ = false;
+    hood_goal_ = 0.6;
+    shooter_velocity_ = 0.0;
+    indexer_angular_velocity_ = 0.0;
+    intake_max_velocity_ = 0.5;
+    InitializeEncoders();
+    ResetDrivetrain();
+    SendSuperstructureGoal();
+  }
+
+  double intake_goal_ = 0.0;
+  double turret_goal_ = 0.0;
+  bool vision_track_ = false;
+  double hood_goal_ = 0.6;
+  double shooter_velocity_ = 0.0;
+  double indexer_angular_velocity_ = 0.0;
+  double intake_max_velocity_ = 0.5;
+
+  void set_intake_goal(double intake_goal) { intake_goal_ = intake_goal; }
+  void set_turret_goal(double turret_goal) { turret_goal_ = turret_goal; }
+  void set_vision_track(bool vision_track) { vision_track_ = vision_track; }
+  void set_hood_goal(double hood_goal) { hood_goal_ = hood_goal; }
+  void set_shooter_velocity(double shooter_velocity) {
+    shooter_velocity_ = shooter_velocity;
+  }
+  void set_indexer_angular_velocity(double indexer_angular_velocity) {
+    indexer_angular_velocity_ = indexer_angular_velocity;
+  }
+  void set_intake_max_velocity(double intake_max_velocity) {
+    intake_max_velocity_ = intake_max_velocity;
+  }
+
+  void SendSuperstructureGoal() {
+    auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+    new_superstructure_goal->intake.distance = intake_goal_;
+    new_superstructure_goal->intake.disable_intake = false;
+    new_superstructure_goal->turret.angle = turret_goal_;
+    new_superstructure_goal->turret.track = vision_track_;
+    new_superstructure_goal->hood.angle = hood_goal_;
+    new_superstructure_goal->shooter.angular_velocity = shooter_velocity_;
+
+    new_superstructure_goal->intake.profile_params.max_velocity =
+        intake_max_velocity_;
+    new_superstructure_goal->turret.profile_params.max_velocity = 6.0;
+    new_superstructure_goal->hood.profile_params.max_velocity = 5.0;
+
+    new_superstructure_goal->intake.profile_params.max_acceleration = 5.0;
+    new_superstructure_goal->turret.profile_params.max_acceleration = 15.0;
+    new_superstructure_goal->hood.profile_params.max_acceleration = 25.0;
+
+    new_superstructure_goal->intake.voltage_rollers = 0.0;
+    new_superstructure_goal->lights_on = true;
+    new_superstructure_goal->intake.disable_intake = false;
+
+    new_superstructure_goal->indexer.angular_velocity =
+        indexer_angular_velocity_;
+
+    if (indexer_angular_velocity_ < -0.1) {
+      new_superstructure_goal->indexer.voltage_rollers = 12.0;
+      new_superstructure_goal->intake.voltage_rollers = 6.0;
+    } else {
+      new_superstructure_goal->indexer.voltage_rollers = 0.0;
+    }
+
+    if (!new_superstructure_goal.Send()) {
+      LOG(ERROR, "Sending superstructure goal failed.\n");
+    }
+  }
 };
 
 }  // namespace actors