commit 9cc9ab62439ccb3991518f6f1ddf68544857d050
author Diana Vandenberg <dkvandenberg2@gmail.com> Wed Apr 20 21:27:47 2016 -0700
committer Austin Schuh <austin.linux@gmail.com> Fri May 13 20:24:50 2016 -0700
tree 543ce475bf147f35155d7c0c9b8d2277da7269af
parent 71466fee065243bcad915ccc5f7fcb214030fc45

Initial stab at hanging actor.

Change-Id: I7d225176962759f8cbcdd42e72fe21eb003349a2

y2016/actors/superstructure_actor.cc

diff --git a/y2016/actors/superstructure_actor.cc b/y2016/actors/superstructure_actor.cc
index b8ad33a..3b0c258 100644
--- a/y2016/actors/superstructure_actor.cc
+++ b/y2016/actors/superstructure_actor.cc
@@ -1,5 +1,7 @@
 #include "y2016/actors/superstructure_actor.h"
 
+#include <cmath>
+
 #include "aos/common/util/phased_loop.h"
 #include "aos/common/logging/logging.h"
 #include "y2016/actors/superstructure_actor.h"
@@ -9,22 +11,99 @@
 namespace actors {
 
 SuperstructureActor::SuperstructureActor(
-    actors::SuperstructureActionQueueGroup* s)
+    actors::SuperstructureActionQueueGroup *s)
     : aos::common::actions::ActorBase<actors::SuperstructureActionQueueGroup>(
           s) {}
 
 bool SuperstructureActor::RunAction(
-    const actors::SuperstructureActionParams& params) {
-  LOG(INFO, "Starting superstructure action with value %f", params.value);
+    const actors::SuperstructureActionParams &params) {
+  LOG(INFO, "Starting superstructure action\n");
 
-  while (true) {
-    control_loops::superstructure_queue.status.FetchLatest();
-    ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
-                                       ::aos::time::Time::InMS(5) / 2);
-    break;
+  MoveSuperstructure(params.partial_angle, false);
+  WaitForSuperstructure();
+  if (ShouldCancel()) return true;
+  MoveSuperstructure(params.partial_angle, true);
+  if (!WaitOrCancel(::aos::time::Time::InSeconds(params.delay_time))) return true;
+  MoveSuperstructure(params.full_angle, true);
+  WaitForSuperstructure();
+  if (ShouldCancel()) return true;
+  return true;
+}
+
+void SuperstructureActor::MoveSuperstructure(double shoulder,
+                                             bool unfold_climber) {
+  superstructure_goal_ = {0, shoulder, 0};
+
+  auto new_superstructure_goal =
+      ::y2016::control_loops::superstructure_queue.goal.MakeMessage();
+
+  new_superstructure_goal->angle_intake = 0;
+  new_superstructure_goal->angle_shoulder = shoulder;
+  new_superstructure_goal->angle_wrist = 0;
+
+  new_superstructure_goal->max_angular_velocity_intake = 7.0;
+  new_superstructure_goal->max_angular_velocity_shoulder = 4.0;
+  new_superstructure_goal->max_angular_velocity_wrist = 10.0;
+
+  new_superstructure_goal->max_angular_acceleration_intake = 40.0;
+  new_superstructure_goal->max_angular_acceleration_shoulder = 5.0;
+  new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
+
+  new_superstructure_goal->voltage_top_rollers = 0;
+  new_superstructure_goal->voltage_bottom_rollers = 0;
+
+  new_superstructure_goal->traverse_unlatched = true;
+  new_superstructure_goal->traverse_down = false;
+  new_superstructure_goal->voltage_climber = 0.0;
+  new_superstructure_goal->unfold_climber = unfold_climber;
+
+  if (!new_superstructure_goal.Send()) {
+    LOG(ERROR, "Sending superstructure move failed.\n");
+  }
+}
+
+bool SuperstructureActor::SuperstructureProfileDone() {
+  constexpr double kProfileError = 1e-2;
+  return ::std::abs(
+             control_loops::superstructure_queue.status->intake.goal_angle -
+             superstructure_goal_.intake) < kProfileError &&
+         ::std::abs(
+             control_loops::superstructure_queue.status->shoulder.goal_angle -
+             superstructure_goal_.shoulder) < kProfileError &&
+         ::std::abs(
+             control_loops::superstructure_queue.status->wrist.goal_angle -
+             superstructure_goal_.wrist) < kProfileError &&
+         ::std::abs(control_loops::superstructure_queue.status->intake
+                        .goal_angular_velocity) < kProfileError &&
+         ::std::abs(control_loops::superstructure_queue.status->shoulder
+                        .goal_angular_velocity) < kProfileError &&
+         ::std::abs(control_loops::superstructure_queue.status->wrist
+                        .goal_angular_velocity) < kProfileError;
+}
+
+bool SuperstructureActor::SuperstructureDone() {
+  control_loops::superstructure_queue.status.FetchAnother();
+
+  // We are no longer running if we are in the zeroing states (below 12), or
+  // estopped.
+  if (control_loops::superstructure_queue.status->state < 12 ||
+      control_loops::superstructure_queue.status->state == 16) {
+    LOG(ERROR, "Superstructure no longer running, aborting action\n");
+    return true;
   }
 
-  return true;
+  if (SuperstructureProfileDone()) {
+    LOG(DEBUG, "Profile done.\n");
+      return true;
+  }
+  return false;
+}
+
+void SuperstructureActor::WaitForSuperstructure() {
+  while (true) {
+    if (ShouldCancel()) return;
+    if (SuperstructureDone()) return;
+  }
 }
 
 ::std::unique_ptr<SuperstructureAction> MakeSuperstructureAction(