Move 2015-specific code to its own folder.
Known issues:
-I didn't change the namespace for it, but I am open to discussion
on doing that in a separate change.
-There are a couple of files which should get split out into
year-specific and not-year-specific files to reduce how much needs
to get copied around each year still.
-The control loop python code doesn't yet generate code with the
right #include etc paths.
Change-Id: Iabf078e75107c283247f58a5ffceb4dbd6a0815f
diff --git a/y2015/actors/stack_actor.cc b/y2015/actors/stack_actor.cc
new file mode 100644
index 0000000..539018f
--- /dev/null
+++ b/y2015/actors/stack_actor.cc
@@ -0,0 +1,96 @@
+#include "y2015/actors/stack_actor.h"
+
+#include <math.h>
+
+#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+
+#include "y2015/constants.h"
+#include "y2015/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr ProfileParams kArmWithStackMove{1.75, 4.20};
+constexpr ProfileParams kSlowArmMove{1.3, 1.4};
+constexpr ProfileParams kSlowElevatorMove{1.0, 3.0};
+
+constexpr ProfileParams kFastArmMove{0.8, 4.0};
+constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
+constexpr ProfileParams kReallyFastElevatorMove{1.2, 6.0};
+} // namespace
+
+StackActor::StackActor(StackActionQueueGroup *queues)
+ : FridgeActorBase<StackActionQueueGroup>(queues) {}
+
+bool StackActor::RunAction(const StackParams ¶ms) {
+ const auto &values = constants::GetValues();
+
+ control_loops::fridge_queue.status.FetchLatest();
+ if (!control_loops::fridge_queue.status.get()) {
+ LOG(ERROR, "Got no fridge status packet.\n");
+ return false;
+ }
+
+ // If we are really high, probably have a can. Move over before down.
+ if (control_loops::fridge_queue.status->goal_height >
+ params.over_box_before_place_height + 0.1) {
+ // Set the current stack down on top of the bottom box.
+ DoFridgeProfile(control_loops::fridge_queue.status->goal_height, 0.0,
+ kSlowElevatorMove, kArmWithStackMove, true);
+ if (ShouldCancel()) return true;
+ }
+
+ // Set the current stack down on top of the bottom box.
+ DoFridgeProfile(params.bottom + values.tote_height, 0.0, kSlowElevatorMove,
+ kSlowArmMove, true);
+ if (ShouldCancel()) return true;
+
+ // Clamp.
+ if (!params.only_place) {
+ // Move the claw out of the way only if we are supposed to pick up.
+ bool send_goal = true;
+ control_loops::claw_queue.status.FetchLatest();
+ if (control_loops::claw_queue.status.get()) {
+ if (control_loops::claw_queue.status->goal_angle <
+ params.claw_out_angle) {
+ send_goal = false;
+ }
+ }
+ if (send_goal) {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.claw_out_angle;
+ message->angular_velocity = 0.0;
+ message->intake = 0.0;
+ message->rollers_closed = true;
+ message->max_velocity = 6.0;
+ message->max_acceleration = 10.0;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+ }
+
+ if (params.only_place) {
+ // open grabber for place only
+ DoFridgeProfile(params.bottom + values.tote_height, 0.0, kFastElevatorMove,
+ kFastArmMove, false);
+ // Finish early if we aren't supposed to grab.
+ return true;
+ }
+
+ if (ShouldCancel()) return true;
+ // grab can (if in fang mode the grabber stays closed)
+ DoFridgeProfile(params.bottom, 0.0, kReallyFastElevatorMove, kFastArmMove, true,
+ true, true);
+
+ return true;
+}
+
+::std::unique_ptr<StackAction> MakeStackAction(const StackParams ¶ms) {
+ return ::std::unique_ptr<StackAction>(
+ new StackAction(&::frc971::actors::stack_action, params));
+}
+
+} // namespace actors
+} // namespace frc971