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/actors.gyp b/y2015/actors/actors.gyp
new file mode 100644
index 0000000..f4989f0
--- /dev/null
+++ b/y2015/actors/actors.gyp
@@ -0,0 +1,630 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'binaries',
+ 'type': 'none',
+ 'dependencies': [
+ 'drivetrain_action',
+ 'score_action',
+ 'score_action_test',
+ 'pickup_action',
+ 'stack_action',
+ 'stack_and_lift_action',
+ 'stack_and_hold_action',
+ 'held_to_lift_action',
+ 'can_pickup_action',
+ 'horizontal_can_pickup_action',
+ 'lift_action',
+ 'stack_action_test',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action_queue',
+ 'type': 'static_library',
+ 'sources': ['drivetrain_action.q'],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'drivetrain_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'drivetrain_actor.cc',
+ ],
+ 'dependencies': [
+ 'drivetrain_action_queue',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(EXTERNALS):eigen',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/y2015/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'drivetrain_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'drivetrain_action',
+ 'type': 'executable',
+ 'sources': [
+ 'drivetrain_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'drivetrain_action_queue',
+ 'drivetrain_action_lib',
+ ],
+ },
+ {
+ 'target_name' : 'fridge_profile_lib',
+ 'type' : 'static_library',
+ 'sources' : [
+ 'fridge_profile_lib.cc',
+ ],
+ 'dependencies' : [
+ '<(AOS)/build/aos.gyp:logging_interface',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/control_loops/fridge/fridge.gyp:fridge_queue',
+ ],
+ 'export_dependent_settings' : [
+ '<(AOS)/build/aos.gyp:logging_interface',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/control_loops/fridge/fridge.gyp:fridge_queue',
+ ],
+ },
+ {
+ 'target_name': 'score_action_queue',
+ 'type': 'static_library',
+ 'sources': ['score_action.q'],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'score_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'score_actor.cc',
+ ],
+ 'dependencies': [
+ 'score_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/y2015/control_loops/fridge/fridge.gyp:fridge_queue',
+ '<(EXTERNALS):eigen',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'score_action_queue',
+ '<(EXTERNALS):eigen',
+ ],
+ },
+ {
+ 'target_name': 'score_action',
+ 'type': 'executable',
+ 'sources': [
+ 'score_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'score_action_queue',
+ 'score_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'score_action_test',
+ 'type': 'executable',
+ 'sources': [
+ 'score_actor_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/common.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/control_loops/fridge/fridge.gyp:fridge_queue',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
+ 'score_action_queue',
+ 'score_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'pickup_action_queue',
+ 'type': 'static_library',
+ 'sources': ['pickup_action.q'],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'pickup_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'pickup_actor.cc',
+ ],
+ 'dependencies': [
+ 'pickup_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/y2015/control_loops/claw/claw.gyp:claw_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'pickup_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'pickup_action',
+ 'type': 'executable',
+ 'sources': [
+ 'pickup_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'pickup_action_queue',
+ 'pickup_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'can_pickup_action_queue',
+ 'type': 'static_library',
+ 'sources': ['can_pickup_action.q'],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'can_pickup_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'can_pickup_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'can_pickup_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/y2015/control_loops/claw/claw.gyp:claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ 'fridge_profile_lib',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'can_pickup_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'can_pickup_action',
+ 'type': 'executable',
+ 'sources': [
+ 'can_pickup_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'can_pickup_action_queue',
+ 'can_pickup_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'horizontal_can_pickup_action_queue',
+ 'type': 'static_library',
+ 'sources': ['horizontal_can_pickup_action.q'],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'horizontal_can_pickup_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'horizontal_can_pickup_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'horizontal_can_pickup_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/y2015/control_loops/claw/claw.gyp:claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ 'fridge_profile_lib',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'horizontal_can_pickup_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'horizontal_can_pickup_action',
+ 'type': 'executable',
+ 'sources': [
+ 'horizontal_can_pickup_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'horizontal_can_pickup_action_queue',
+ 'horizontal_can_pickup_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'held_to_lift_action_queue',
+ 'type': 'static_library',
+ 'sources': ['held_to_lift_action.q'],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'lift_action_params',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'lift_action_params',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'held_to_lift_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'held_to_lift_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'held_to_lift_action_queue',
+ 'lift_action_lib',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/y2015/control_loops/claw/claw.gyp:claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'held_to_lift_action_queue',
+ 'fridge_profile_lib',
+ ],
+ },
+ {
+ 'target_name': 'held_to_lift_action',
+ 'type': 'executable',
+ 'sources': [
+ 'held_to_lift_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'held_to_lift_action_queue',
+ 'held_to_lift_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'stack_and_hold_action_queue',
+ 'type': 'static_library',
+ 'sources': ['stack_and_hold_action.q'],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_params',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_params',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'stack_and_hold_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'stack_and_hold_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'stack_and_hold_action_queue',
+ 'stack_action_lib',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/y2015/control_loops/claw/claw.gyp:claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_and_hold_action_queue',
+ 'fridge_profile_lib',
+ ],
+ },
+ {
+ 'target_name': 'stack_and_hold_action',
+ 'type': 'executable',
+ 'sources': [
+ 'stack_and_hold_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_and_hold_action_queue',
+ 'stack_and_hold_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'stack_and_lift_action_queue',
+ 'type': 'static_library',
+ 'sources': ['stack_and_lift_action.q'],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_params',
+ 'lift_action_params',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_params',
+ 'lift_action_params',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'stack_and_lift_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'stack_and_lift_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'stack_and_lift_action_queue',
+ 'stack_action_lib',
+ 'lift_action_lib',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/y2015/control_loops/claw/claw.gyp:claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_and_lift_action_queue',
+ 'fridge_profile_lib',
+ ],
+ },
+ {
+ 'target_name': 'stack_and_lift_action',
+ 'type': 'executable',
+ 'sources': [
+ 'stack_and_lift_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_and_lift_action_queue',
+ 'stack_and_lift_action_lib',
+ ],
+ },
+ {
+ # This is a wrapper target to avoid adding the directory with a stale
+ # stack_action_params.q.h to the compiler's include path.
+ 'target_name': 'stack_action_queue',
+ 'type': 'none',
+ 'dependencies': ['stack_action_queue_real'],
+ 'export_dependent_settings': ['stack_action_queue_real'],
+ },
+ {
+ 'target_name': 'stack_action_queue_real',
+ 'type': 'static_library',
+ 'sources': [
+ 'stack_action.q',
+ ],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_params',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'stack_action_params',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'stack_action_params',
+ 'type': 'static_library',
+ 'sources': [
+ 'stack_action_params.q',
+ ],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'stack_action_test',
+ 'type': 'executable',
+ 'sources': [
+ 'stack_actor_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/common.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/control_loops/fridge/fridge.gyp:fridge_queue',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
+ 'stack_action_queue',
+ 'stack_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'stack_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'stack_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'stack_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/y2015/control_loops/claw/claw.gyp:claw_queue',
+ ],
+ 'export_dependent_settings': [
+ 'fridge_profile_lib',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'stack_action',
+ 'type': 'executable',
+ 'sources': [
+ 'stack_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'stack_action_queue',
+ 'stack_action_lib',
+ ],
+ },
+ {
+ # This is a wrapper target to avoid adding the directory with a stale
+ # lift_action_params.q.h to the compiler's include path.
+ 'target_name': 'lift_action_queue',
+ 'type': 'none',
+ 'dependencies': ['lift_action_queue_real'],
+ 'export_dependent_settings': ['lift_action_queue_real'],
+ },
+ {
+ 'target_name': 'lift_action_queue_real',
+ 'type': 'static_library',
+ 'sources': [
+ 'lift_action.q',
+ ],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'dependencies': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'lift_action_params',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/actions/actions.gyp:action_queue',
+ 'lift_action_params',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'lift_action_params',
+ 'type': 'static_library',
+ 'sources': [
+ 'lift_action_params.q',
+ ],
+ 'variables': {
+ 'header_path': 'y2015/actors',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'lift_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'lift_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_lib',
+ 'lift_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/y2015/y2015.gyp:constants',
+ '<(DEPTH)/y2015/control_loops/claw/claw.gyp:claw_queue',
+ ],
+ 'export_dependent_settings': [
+ 'fridge_profile_lib',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'lift_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'lift_action',
+ 'type': 'executable',
+ 'sources': [
+ 'lift_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'lift_action_queue',
+ 'lift_action_lib',
+ ],
+ },
+ ],
+}
diff --git a/y2015/actors/can_pickup_action.q b/y2015/actors/can_pickup_action.q
new file mode 100644
index 0000000..4846722
--- /dev/null
+++ b/y2015/actors/can_pickup_action.q
@@ -0,0 +1,40 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+// Parameters to send with start.
+// This action picks a right side up can from the claw.
+// It starts by lifting up, then moving the arm out, then lifting the can out of
+// the claw, and then ends with the can inside the bot.
+struct CanPickupParams {
+ // X position for the fridge when picking up.
+ double pickup_x;
+ // Y position for the fridge when picking up.
+ double pickup_y;
+ // Height to move the elevator to to lift the can out of the claw.
+ double lift_height;
+ // Height to use when lifting before moving it back.
+ double pickup_goal_before_move_height;
+ // The height at which to start pulling back.
+ double before_place_height;
+
+ // X at which to start lowering the can.
+ double start_lowering_x;
+ // End position with the can.
+ double end_height;
+ double end_angle;
+};
+
+queue_group CanPickupActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ CanPickupParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group CanPickupActionQueueGroup can_pickup_action;
diff --git a/y2015/actors/can_pickup_actor.cc b/y2015/actors/can_pickup_actor.cc
new file mode 100644
index 0000000..d2a191f
--- /dev/null
+++ b/y2015/actors/can_pickup_actor.cc
@@ -0,0 +1,122 @@
+#include <math.h>
+
+#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+
+#include "y2015/actors/can_pickup_actor.h"
+#include "y2015/constants.h"
+#include "y2015/control_loops/claw/claw.q.h"
+
+using ::frc971::control_loops::fridge_queue;
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr ProfileParams kHorizontalMove{1.1, 1.8};
+constexpr ProfileParams kVerticalMove{0.3, 2.0};
+constexpr ProfileParams kFastHorizontalMove{1.25, 5.0};
+constexpr ProfileParams kFastVerticalMove{0.40, 2.0};
+constexpr ProfileParams kPureVerticalMove{1.20, 5.0};
+} // namespace
+
+CanPickupActor::CanPickupActor(CanPickupActionQueueGroup *queues)
+ : FridgeActorBase<CanPickupActionQueueGroup>(queues) {}
+
+double CanPickupActor::CurrentGoalX() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ return fridge_queue.status->goal_x;
+}
+
+double CanPickupActor::CurrentGoalHeight() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ return fridge_queue.status->goal_y;
+}
+
+bool CanPickupActor::RunAction(const CanPickupParams ¶ms) {
+ // Make sure the claw is down.
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = 0.0;
+ 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 down goal", *message);
+ message.Send();
+ }
+
+ // Go around the can.
+ DoFridgeXYProfile(params.pickup_x, params.pickup_y, kFastHorizontalMove,
+ kFastVerticalMove, true, false, false);
+ if (ShouldCancel()) return true;
+
+ if (!StartFridgeXYProfile(
+ params.pickup_x, params.pickup_goal_before_move_height,
+ kHorizontalMove, kPureVerticalMove, true, true, true)) {
+ return false;
+ }
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = 0.0;
+ message->angular_velocity = 0.0;
+ message->intake = 0.0;
+ message->rollers_closed = false;
+ message->max_velocity = 6.0;
+ message->max_acceleration = 10.0;
+
+ LOG_STRUCT(DEBUG, "Sending claw down goal", *message);
+ message.Send();
+ }
+
+ bool above_claw = false;
+ while (true) {
+ if (CurrentGoalHeight() > params.lift_height && !above_claw) {
+ if (!StartFridgeXYProfile(0.0, params.before_place_height,
+ kHorizontalMove, kVerticalMove, true, true,
+ true)) {
+ return false;
+ }
+ above_claw = true;
+ }
+ if (CurrentGoalX() < params.start_lowering_x) {
+ // Getting close, start lowering.
+ LOG(DEBUG, "Starting to lower the can onto the tray.\n");
+ break;
+ }
+ ProfileStatus status =
+ IterateXYProfile(0.0, params.before_place_height, kHorizontalMove,
+ kVerticalMove, true, true, true);
+ if (status == DONE || status == CANCELED) {
+ break;
+ }
+ }
+ if (ShouldCancel()) return true;
+
+ // Lower it.
+ DoFridgeXYProfile(0.0, params.end_height, kHorizontalMove, kPureVerticalMove,
+ true);
+ if (ShouldCancel()) return true;
+
+ return true;
+}
+
+::std::unique_ptr<CanPickupAction> MakeCanPickupAction(
+ const CanPickupParams ¶ms) {
+ return ::std::unique_ptr<CanPickupAction>(
+ new CanPickupAction(&::frc971::actors::can_pickup_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/can_pickup_actor.h b/y2015/actors/can_pickup_actor.h
new file mode 100644
index 0000000..f0b1ec9
--- /dev/null
+++ b/y2015/actors/can_pickup_actor.h
@@ -0,0 +1,37 @@
+#ifndef Y2015_ACTORS_CAN_PICKUP_ACTOR_H_
+#define Y2015_ACTORS_CAN_PICKUP_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/can_pickup_action.q.h"
+#include "y2015/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class CanPickupActor : public FridgeActorBase<CanPickupActionQueueGroup> {
+ public:
+ explicit CanPickupActor(CanPickupActionQueueGroup *queues);
+
+ bool RunAction(const CanPickupParams ¶ms) override;
+
+ private:
+ double CurrentGoalHeight();
+ double CurrentGoalX();
+};
+
+typedef aos::common::actions::TypedAction<CanPickupActionQueueGroup>
+ CanPickupAction;
+
+// Makes a new CanPickupActor action.
+::std::unique_ptr<CanPickupAction> MakeCanPickupAction(
+ const CanPickupParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif
diff --git a/y2015/actors/can_pickup_actor_main.cc b/y2015/actors/can_pickup_actor_main.cc
new file mode 100644
index 0000000..bed9412
--- /dev/null
+++ b/y2015/actors/can_pickup_actor_main.cc
@@ -0,0 +1,15 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/can_pickup_action.q.h"
+#include "y2015/actors/can_pickup_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::CanPickupActor can_pickup(&::frc971::actors::can_pickup_action);
+ can_pickup.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/actors/drivetrain_action.q b/y2015/actors/drivetrain_action.q
new file mode 100644
index 0000000..9ad55d3
--- /dev/null
+++ b/y2015/actors/drivetrain_action.q
@@ -0,0 +1,29 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+// Parameters to send with start.
+struct DrivetrainActionParams {
+ double left_initial_position;
+ double right_initial_position;
+ double y_offset;
+ double theta_offset;
+ double maximum_velocity;
+ double maximum_acceleration;
+ double maximum_turn_velocity;
+ double maximum_turn_acceleration;
+};
+
+queue_group DrivetrainActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ DrivetrainActionParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/y2015/actors/drivetrain_actor.cc b/y2015/actors/drivetrain_actor.cc
new file mode 100644
index 0000000..9ef81c5
--- /dev/null
+++ b/y2015/actors/drivetrain_actor.cc
@@ -0,0 +1,168 @@
+#include "y2015/actors/drivetrain_actor.h"
+
+#include <functional>
+#include <numeric>
+
+#include <Eigen/Dense>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/time.h"
+
+#include "y2015/actors/drivetrain_actor.h"
+#include "y2015/constants.h"
+#include "y2015/control_loops/drivetrain/drivetrain.q.h"
+
+namespace frc971 {
+namespace actors {
+
+DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
+ : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
+
+bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
+ static const auto K = constants::GetValues().make_drivetrain_loop().K();
+
+ const double yoffset = params.y_offset;
+ const double turn_offset =
+ params.theta_offset * constants::GetValues().turn_width / 2.0;
+ LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
+
+ // Measured conversion to get the distance right.
+ ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
+ const double goal_velocity = 0.0;
+ const double epsilon = 0.01;
+ ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
+
+ profile.set_maximum_acceleration(params.maximum_acceleration);
+ profile.set_maximum_velocity(params.maximum_velocity);
+ turn_profile.set_maximum_acceleration(params.maximum_turn_acceleration *
+ constants::GetValues().turn_width /
+ 2.0);
+ turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
+ constants::GetValues().turn_width / 2.0);
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(5, 2500);
+
+ control_loops::drivetrain_queue.status.FetchLatest();
+ if (control_loops::drivetrain_queue.status.get()) {
+ const auto& status = *control_loops::drivetrain_queue.status;
+ if (::std::abs(status.uncapped_left_voltage -
+ status.uncapped_right_voltage) > 24) {
+ LOG(DEBUG, "spinning in place\n");
+ // They're more than 24V apart, so stop moving forwards and let it deal
+ // with spinning first.
+ profile.SetGoal(
+ (status.filtered_left_position + status.filtered_right_position -
+ params.left_initial_position - params.right_initial_position) /
+ 2.0);
+ } else {
+ static const double divisor = K(0, 0) + K(0, 2);
+ double dx_left, dx_right;
+ if (status.uncapped_left_voltage > 12.0) {
+ dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
+ } else if (status.uncapped_left_voltage < -12.0) {
+ dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
+ } else {
+ dx_left = 0;
+ }
+ if (status.uncapped_right_voltage > 12.0) {
+ dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
+ } else if (status.uncapped_right_voltage < -12.0) {
+ dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
+ } else {
+ dx_right = 0;
+ }
+ double dx;
+ if (dx_left == 0 && dx_right == 0) {
+ dx = 0;
+ } else if (dx_left != 0 && dx_right != 0 &&
+ ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
+ // Both saturating in opposite directions. Don't do anything.
+ LOG(DEBUG, "Saturating opposite ways, not adjusting\n");
+ dx = 0;
+ } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
+ dx = dx_left;
+ } else {
+ dx = dx_right;
+ }
+ if (dx != 0) {
+ LOG(DEBUG, "adjusting goal by %f\n", dx);
+ profile.MoveGoal(-dx);
+ }
+ }
+ } else {
+ // If we ever get here, that's bad and we should just give up
+ LOG(ERROR, "no drivetrain status!\n");
+ return false;
+ }
+
+ const auto drive_profile_goal_state =
+ profile.Update(yoffset, goal_velocity);
+ const auto turn_profile_goal_state = turn_profile.Update(turn_offset, 0.0);
+ left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
+ right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
+
+ if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
+ ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
+ break;
+ }
+
+ if (ShouldCancel()) return true;
+
+ LOG(DEBUG, "Driving left to %f, right to %f\n",
+ left_goal_state(0, 0) + params.left_initial_position,
+ right_goal_state(0, 0) + params.right_initial_position);
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ //.highgear(false)
+ .left_goal(left_goal_state(0, 0) + params.left_initial_position)
+ .right_goal(right_goal_state(0, 0) + params.right_initial_position)
+ .left_velocity_goal(left_goal_state(1, 0))
+ .right_velocity_goal(right_goal_state(1, 0))
+ .Send();
+ }
+ if (ShouldCancel()) return true;
+ control_loops::drivetrain_queue.status.FetchLatest();
+ while (!control_loops::drivetrain_queue.status.get()) {
+ LOG(WARNING,
+ "No previous drivetrain status packet, trying to fetch again\n");
+ control_loops::drivetrain_queue.status.FetchNextBlocking();
+ if (ShouldCancel()) return true;
+ }
+ while (true) {
+ if (ShouldCancel()) return true;
+ const double kPositionThreshold = 0.05;
+
+ const double left_error = ::std::abs(
+ control_loops::drivetrain_queue.status->filtered_left_position -
+ (left_goal_state(0, 0) + params.left_initial_position));
+ const double right_error = ::std::abs(
+ control_loops::drivetrain_queue.status->filtered_right_position -
+ (right_goal_state(0, 0) + params.right_initial_position));
+ const double velocity_error =
+ ::std::abs(control_loops::drivetrain_queue.status->robot_speed);
+ if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
+ velocity_error < 0.2) {
+ break;
+ } else {
+ LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
+ velocity_error);
+ }
+ control_loops::drivetrain_queue.status.FetchNextBlocking();
+ }
+ LOG(INFO, "Done moving\n");
+ return true;
+}
+
+::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
+ const ::frc971::actors::DrivetrainActionParams& params) {
+ return ::std::unique_ptr<DrivetrainAction>(
+ new DrivetrainAction(&::frc971::actors::drivetrain_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/drivetrain_actor.h b/y2015/actors/drivetrain_actor.h
new file mode 100644
index 0000000..369c6ed
--- /dev/null
+++ b/y2015/actors/drivetrain_actor.h
@@ -0,0 +1,31 @@
+#ifndef Y2015_ACTIONS_DRIVETRAIN_ACTION_H_
+#define Y2015_ACTIONS_DRIVETRAIN_ACTION_H_
+
+#include <memory>
+
+#include "y2015/actors/drivetrain_action.q.h"
+#include "aos/common/actions/actor.h"
+#include "aos/common/actions/actions.h"
+
+namespace frc971 {
+namespace actors {
+
+class DrivetrainActor
+ : public aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
+ public:
+ explicit DrivetrainActor(DrivetrainActionQueueGroup* s);
+
+ bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<DrivetrainActionQueueGroup>
+ DrivetrainAction;
+
+// Makes a new DrivetrainActor action.
+::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
+ const ::frc971::actors::DrivetrainActionParams& params);
+
+} // namespace actors
+} // namespace frc971
+
+#endif
diff --git a/y2015/actors/drivetrain_actor_main.cc b/y2015/actors/drivetrain_actor_main.cc
new file mode 100644
index 0000000..7e461ff
--- /dev/null
+++ b/y2015/actors/drivetrain_actor_main.cc
@@ -0,0 +1,18 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/drivetrain_action.q.h"
+#include "y2015/actors/drivetrain_actor.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actors::DrivetrainActor drivetrain(
+ &::frc971::actors::drivetrain_action);
+ drivetrain.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/actors/fridge_profile_lib.cc b/y2015/actors/fridge_profile_lib.cc
new file mode 100644
index 0000000..1a1866f
--- /dev/null
+++ b/y2015/actors/fridge_profile_lib.cc
@@ -0,0 +1 @@
+#include "y2015/actors/fridge_profile_lib.h"
diff --git a/y2015/actors/fridge_profile_lib.h b/y2015/actors/fridge_profile_lib.h
new file mode 100644
index 0000000..ce75f75
--- /dev/null
+++ b/y2015/actors/fridge_profile_lib.h
@@ -0,0 +1,285 @@
+#ifndef Y2015_ACTORS_FRIDGE_PROFILE_LIB_H_
+#define Y2015_ACTORS_FRIDGE_PROFILE_LIB_H_
+
+#include <cmath>
+
+#include "aos/common/actions/actor.h"
+#include "aos/common/util/phased_loop.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
+
+namespace frc971 {
+namespace actors {
+
+struct ProfileParams {
+ double velocity;
+ double acceleration;
+};
+
+// Base class to provide helper utilities to all Actors who want to control the
+// fridge.
+template <typename T>
+class FridgeActorBase : public aos::common::actions::ActorBase<T> {
+ public:
+ FridgeActorBase(T *queues) : aos::common::actions::ActorBase<T>(queues) {}
+
+ protected:
+ void DoFridgeProfile(double height, double angle,
+ ProfileParams elevator_parameters,
+ ProfileParams arm_parameters, bool grabbers) {
+ DoFridgeProfile(height, angle, elevator_parameters, arm_parameters,
+ grabbers, grabbers, grabbers);
+ }
+
+ bool StartFridgeProfile(double height, double angle,
+ ProfileParams elevator_parameters,
+ ProfileParams arm_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 0;
+ new_fridge_goal->max_velocity = elevator_parameters.velocity;
+ new_fridge_goal->max_acceleration = elevator_parameters.acceleration;
+ new_fridge_goal->height = height;
+ new_fridge_goal->velocity = 0.0;
+ new_fridge_goal->max_angular_velocity = arm_parameters.velocity;
+ new_fridge_goal->max_angular_acceleration = arm_parameters.acceleration;
+ new_fridge_goal->angle = angle;
+ new_fridge_goal->angular_velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+ LOG(INFO, "Starting profile to %f, %f\n", height, angle);
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ return false;
+ }
+ return true;
+ }
+
+ enum ProfileStatus { RUNNING, DONE, CANCELED };
+
+ ProfileStatus IterateProfile(double height, double angle,
+ ProfileParams elevator_parameters,
+ ProfileParams arm_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (this->ShouldCancel()) {
+ LOG(INFO, "Canceling fridge movement\n");
+ if (!control_loops::fridge_queue.status.get()) {
+ LOG(WARNING, "no fridge status so can't really cancel\n");
+ return CANCELED;
+ }
+
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 0;
+ new_fridge_goal->max_velocity = elevator_parameters.velocity;
+ new_fridge_goal->max_acceleration = elevator_parameters.acceleration;
+ new_fridge_goal->height =
+ control_loops::fridge_queue.status->height +
+ (control_loops::fridge_queue.status->goal_velocity *
+ ::std::abs(control_loops::fridge_queue.status->goal_velocity)) /
+ (2.0 * new_fridge_goal->max_acceleration);
+ height = new_fridge_goal->height;
+ new_fridge_goal->velocity = 0.0;
+ new_fridge_goal->max_angular_velocity = arm_parameters.velocity;
+ new_fridge_goal->max_angular_acceleration = arm_parameters.acceleration;
+ new_fridge_goal->angle =
+ control_loops::fridge_queue.status->angle +
+ (control_loops::fridge_queue.status->goal_angular_velocity *
+ ::std::abs(
+ control_loops::fridge_queue.status->goal_angular_velocity)) /
+ (2.0 * new_fridge_goal->max_angular_acceleration);
+ angle = new_fridge_goal->angle;
+ new_fridge_goal->angular_velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ }
+ return CANCELED;
+ }
+ control_loops::fridge_queue.status.FetchAnother();
+
+ constexpr double kProfileError = 1e-5;
+ constexpr double kAngleEpsilon = 0.02, kHeightEpsilon = 0.015;
+
+ if (control_loops::fridge_queue.status->state != 4) {
+ LOG(ERROR, "Fridge no longer running, aborting action\n");
+ return CANCELED;
+ }
+
+ if (::std::abs(control_loops::fridge_queue.status->goal_angle - angle) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_height - height) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_angular_velocity) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_velocity) <
+ kProfileError) {
+ LOG(INFO, "Profile done.\n");
+ if (::std::abs(control_loops::fridge_queue.status->angle - angle) <
+ kAngleEpsilon &&
+ ::std::abs(control_loops::fridge_queue.status->height -
+ height) < kHeightEpsilon) {
+ LOG(INFO, "Near goal, done.\n");
+ return DONE;
+ }
+ }
+
+ return RUNNING;
+ }
+
+ void DoFridgeProfile(double height, double angle,
+ ProfileParams elevator_parameters,
+ ProfileParams arm_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (!StartFridgeProfile(height, angle, elevator_parameters, arm_parameters,
+ top_grabbers, front_grabbers, back_grabbers)) {
+ return;
+ }
+
+ while (true) {
+ ProfileStatus status =
+ IterateProfile(height, angle, elevator_parameters, arm_parameters,
+ top_grabbers, front_grabbers, back_grabbers);
+ if (status == DONE || status == CANCELED) {
+ return;
+ }
+ }
+ }
+
+ void DoFridgeXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool grabbers) {
+ DoFridgeXYProfile(x, y, x_parameters, y_parameters, grabbers, grabbers,
+ grabbers);
+ }
+
+ void DoFridgeXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (!StartFridgeXYProfile(x, y, x_parameters, y_parameters, top_grabbers,
+ front_grabbers, back_grabbers)) {
+ return;
+ }
+
+ while (true) {
+ ProfileStatus status =
+ IterateXYProfile(x, y, x_parameters, y_parameters, top_grabbers,
+ front_grabbers, back_grabbers);
+ if (status == DONE || status == CANCELED) {
+ return;
+ }
+ }
+ }
+
+ void CancelXYMotion(ProfileParams x_parameters, ProfileParams y_parameters,
+ bool top_grabbers, bool front_grabbers,
+ bool back_grabbers) {
+ LOG(INFO, "Canceling fridge movement\n");
+ if (!control_loops::fridge_queue.status.get()) {
+ LOG(WARNING, "no fridge status so can't really cancel\n");
+ return;
+ }
+
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 1;
+ new_fridge_goal->max_x_velocity = x_parameters.velocity;
+ new_fridge_goal->max_x_acceleration = x_parameters.acceleration;
+ new_fridge_goal->x =
+ control_loops::fridge_queue.status->x +
+ (control_loops::fridge_queue.status->goal_x_velocity *
+ ::std::abs(control_loops::fridge_queue.status->goal_x_velocity)) /
+ (2.0 * new_fridge_goal->max_x_acceleration);
+ new_fridge_goal->x_velocity = 0.0;
+
+ new_fridge_goal->max_y_velocity = y_parameters.velocity;
+ new_fridge_goal->max_y_acceleration = y_parameters.acceleration;
+ new_fridge_goal->y =
+ control_loops::fridge_queue.status->y +
+ (control_loops::fridge_queue.status->goal_y_velocity *
+ ::std::abs(control_loops::fridge_queue.status->goal_y_velocity)) /
+ (2.0 * new_fridge_goal->max_y_acceleration);
+ new_fridge_goal->y_velocity = 0.0;
+
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ }
+ }
+
+ ProfileStatus IterateXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ if (this->ShouldCancel()) {
+ CancelXYMotion(x_parameters, y_parameters, top_grabbers, front_grabbers,
+ back_grabbers);
+ return CANCELED;
+ }
+ control_loops::fridge_queue.status.FetchAnother();
+
+ constexpr double kProfileError = 1e-5;
+ constexpr double kXEpsilon = 0.02, kYEpsilon = 0.02;
+
+ if (control_loops::fridge_queue.status->state != 4) {
+ LOG(ERROR, "Fridge no longer running, aborting action\n");
+ return CANCELED;
+ }
+
+ if (::std::abs(control_loops::fridge_queue.status->goal_x - x) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_y - y) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_x_velocity) <
+ kProfileError &&
+ ::std::abs(control_loops::fridge_queue.status->goal_y_velocity) <
+ kProfileError) {
+ LOG(INFO, "Profile done.\n");
+ if (::std::abs(control_loops::fridge_queue.status->x - x) < kXEpsilon &&
+ ::std::abs(control_loops::fridge_queue.status->y - y) < kYEpsilon) {
+ LOG(INFO, "Near goal, done.\n");
+ return DONE;
+ }
+ }
+
+ return RUNNING;
+ }
+
+ bool StartFridgeXYProfile(double x, double y, ProfileParams x_parameters,
+ ProfileParams y_parameters, bool top_grabbers,
+ bool front_grabbers, bool back_grabbers) {
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->profiling_type = 1;
+ new_fridge_goal->max_x_velocity = x_parameters.velocity;
+ new_fridge_goal->max_x_acceleration = x_parameters.acceleration;
+ new_fridge_goal->x = x;
+ new_fridge_goal->x_velocity = 0.0;
+
+ new_fridge_goal->max_y_velocity = y_parameters.velocity;
+ new_fridge_goal->max_y_acceleration = y_parameters.acceleration;
+ new_fridge_goal->y = y;
+ new_fridge_goal->y_velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = top_grabbers;
+ new_fridge_goal->grabbers.top_back = top_grabbers;
+ new_fridge_goal->grabbers.bottom_front = front_grabbers;
+ new_fridge_goal->grabbers.bottom_back = back_grabbers;
+ LOG(INFO, "Starting xy profile to %f, %f\n", x, y);
+
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge goal\n");
+ return false;
+ }
+ return true;
+ }
+};
+
+} // namespace actors
+} // namespace frc971
+
+#endif // Y2015_ACTORS_FRIDGE_PROFILE_LIB_H_
diff --git a/y2015/actors/held_to_lift_action.q b/y2015/actors/held_to_lift_action.q
new file mode 100644
index 0000000..c2dd689
--- /dev/null
+++ b/y2015/actors/held_to_lift_action.q
@@ -0,0 +1,36 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+import "y2015/actors/lift_action_params.q";
+
+// Parameters to send with start.
+struct HeldToLiftParams {
+ // The maximum claw value to avoid collisions.
+ double claw_out_angle;
+
+ // The value to move the arm forwards to clear the stack when lowering.
+ double arm_clearance;
+ // End height.
+ double bottom_height;
+ // Amount to wait for the elevator to settle before lifting.
+ double before_lift_settle_time;
+ // Amount to wait to clamp.
+ double clamp_pause_time;
+
+ // Lift parameters
+ LiftParams lift_params;
+};
+
+queue_group HeldToLiftActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ HeldToLiftParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group HeldToLiftActionQueueGroup held_to_lift_action;
diff --git a/y2015/actors/held_to_lift_actor.cc b/y2015/actors/held_to_lift_actor.cc
new file mode 100644
index 0000000..2336364
--- /dev/null
+++ b/y2015/actors/held_to_lift_actor.cc
@@ -0,0 +1,114 @@
+#include "y2015/actors/held_to_lift_actor.h"
+
+#include <math.h>
+
+#include "aos/common/time.h"
+#include "y2015/constants.h"
+#include "y2015/actors/fridge_profile_lib.h"
+#include "y2015/actors/lift_actor.h"
+#include "y2015/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr ProfileParams kArmMove{0.6, 2.0};
+constexpr ProfileParams kFastArmMove{1.2, 4.0};
+constexpr ProfileParams kElevatorMove{0.9, 3.0};
+constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
+} // namespace
+
+HeldToLiftActor::HeldToLiftActor(HeldToLiftActionQueueGroup *queues)
+ : FridgeActorBase<HeldToLiftActionQueueGroup>(queues) {}
+
+bool HeldToLiftActor::RunAction(const HeldToLiftParams ¶ms) {
+ control_loops::fridge_queue.status.FetchLatest();
+ if (!control_loops::fridge_queue.status.get()) {
+ return false;
+ }
+
+ // Move claw out of the way.
+ {
+ bool send_goal = true;
+ double claw_goal = params.claw_out_angle;
+ control_loops::claw_queue.status.FetchLatest();
+ if (control_loops::claw_queue.status.get()) {
+ if (control_loops::claw_queue.status->goal_angle < claw_goal) {
+ 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->max_velocity = 6.0;
+ message->max_acceleration = 10.0;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+ }
+
+ control_loops::fridge_queue.status.FetchLatest();
+ if (!control_loops::fridge_queue.status.get()) {
+ return false;
+ }
+
+ if (control_loops::fridge_queue.status->goal_height != params.bottom_height ||
+ control_loops::fridge_queue.status->goal_angle != 0.0) {
+ // Lower with the fridge clamps open and move it forwards slightly to clear.
+ DoFridgeProfile(control_loops::fridge_queue.status->goal_height,
+ params.arm_clearance, kFastElevatorMove, kFastArmMove,
+ false);
+ if (ShouldCancel()) return true;
+
+ DoFridgeProfile(params.bottom_height, params.arm_clearance,
+ kFastElevatorMove, kFastArmMove, false);
+ if (ShouldCancel()) return true;
+
+ // Move it back to the storage location.
+ DoFridgeProfile(params.bottom_height, 0.0, kElevatorMove, kArmMove, false);
+ if (ShouldCancel()) return true;
+
+ if (!WaitOrCancel(
+ aos::time::Time::InSeconds(params.before_lift_settle_time))) {
+ return true;
+ }
+
+ // Clamp
+ DoFridgeProfile(params.bottom_height, 0.0, kElevatorMove, kArmMove, true);
+ if (ShouldCancel()) return true;
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.clamp_pause_time))) {
+ return true;
+ }
+ }
+
+ {
+ ::std::unique_ptr<LiftAction> lift_action =
+ MakeLiftAction(params.lift_params);
+ lift_action->Start();
+ while (lift_action->Running()) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ 2500);
+
+ if (ShouldCancel()) {
+ lift_action->Cancel();
+ LOG(WARNING, "Cancelling fridge and claw.\n");
+ return true;
+ }
+ }
+ }
+
+ return true;
+}
+
+::std::unique_ptr<HeldToLiftAction> MakeHeldToLiftAction(
+ const HeldToLiftParams ¶ms) {
+ return ::std::unique_ptr<HeldToLiftAction>(
+ new HeldToLiftAction(&::frc971::actors::held_to_lift_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/held_to_lift_actor.h b/y2015/actors/held_to_lift_actor.h
new file mode 100644
index 0000000..369fea4
--- /dev/null
+++ b/y2015/actors/held_to_lift_actor.h
@@ -0,0 +1,33 @@
+#ifndef Y2015_ACTORS_HELD_TO_LIFT_ACTOR_H_
+#define Y2015_ACTORS_HELD_TO_LIFT_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/held_to_lift_action.q.h"
+#include "y2015/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class HeldToLiftActor : public FridgeActorBase<HeldToLiftActionQueueGroup> {
+ public:
+ explicit HeldToLiftActor(HeldToLiftActionQueueGroup *queues);
+
+ bool RunAction(const HeldToLiftParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<HeldToLiftActionQueueGroup>
+ HeldToLiftAction;
+
+// Makes a new HeldToLiftActor action.
+::std::unique_ptr<HeldToLiftAction> MakeHeldToLiftAction(
+ const HeldToLiftParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // Y2015_ACTORS_HELD_TO_LIFT_ACTOR_H_
diff --git a/y2015/actors/held_to_lift_actor_main.cc b/y2015/actors/held_to_lift_actor_main.cc
new file mode 100644
index 0000000..a5d78e1
--- /dev/null
+++ b/y2015/actors/held_to_lift_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/held_to_lift_action.q.h"
+#include "y2015/actors/held_to_lift_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::HeldToLiftActor lift(
+ &::frc971::actors::held_to_lift_action);
+ lift.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/actors/horizontal_can_pickup_action.q b/y2015/actors/horizontal_can_pickup_action.q
new file mode 100644
index 0000000..c1a1c55
--- /dev/null
+++ b/y2015/actors/horizontal_can_pickup_action.q
@@ -0,0 +1,48 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+// Parameters to send with start.
+// This action picks a horizontal can from the claw.
+struct HorizontalCanPickupParams {
+ // Elevator catch height.
+ double elevator_height;
+ // Angle to move the claw to when placing the base of the can on the robot.
+ double pickup_angle;
+
+ // Time and power to spit the can out before lifting.
+ double spit_time;
+ double spit_power;
+
+ // Time and power to pull the can in when lifted.
+ double suck_time;
+ double suck_power;
+
+ // Time to push down and suck in to slide the claw down on the can.
+ double claw_settle_time;
+ double claw_settle_power;
+
+ // Angle to lift the claw to to lift the can.
+ double claw_full_lift_angle;
+
+ // Angle to move the claw back down to.
+ double claw_end_angle;
+
+ // The end arm and elevator position once we are done lifting.
+ double elevator_end_height;
+ double arm_end_angle;
+};
+
+queue_group HorizontalCanPickupActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ HorizontalCanPickupParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group HorizontalCanPickupActionQueueGroup horizontal_can_pickup_action;
diff --git a/y2015/actors/horizontal_can_pickup_actor.cc b/y2015/actors/horizontal_can_pickup_actor.cc
new file mode 100644
index 0000000..fb6e542
--- /dev/null
+++ b/y2015/actors/horizontal_can_pickup_actor.cc
@@ -0,0 +1,161 @@
+#include <math.h>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+
+#include "y2015/actors/horizontal_can_pickup_actor.h"
+#include "y2015/actors/fridge_profile_lib.h"
+#include "y2015/constants.h"
+#include "y2015/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr ProfileParams kClawPickup{3.0, 2.0};
+constexpr ProfileParams kClawBackDown{7.0, 10.0};
+constexpr ProfileParams kClawInitialLift{7.0, 8.0};
+
+constexpr ProfileParams kArmMove{1.0, 1.6};
+constexpr ProfileParams kElevatorMove{0.6, 2.2};
+
+constexpr ProfileParams kFastArmMove{2.0, 3.0};
+constexpr ProfileParams kFastElevatorMove{1.0, 3.0};
+
+constexpr double kAngleEpsilon = 0.10;
+constexpr double kGoalAngleEpsilon = 0.01;
+
+} // namespace
+
+HorizontalCanPickupActor::HorizontalCanPickupActor(
+ HorizontalCanPickupActionQueueGroup *queues)
+ : FridgeActorBase<HorizontalCanPickupActionQueueGroup>(queues) {}
+
+bool HorizontalCanPickupActor::WaitUntilGoalNear(double angle) {
+ while (true) {
+ control_loops::claw_queue.status.FetchAnother();
+ if (ShouldCancel()) return false;
+ const double goal_angle = control_loops::claw_queue.status->goal_angle;
+ LOG_STRUCT(DEBUG, "Got claw status", *control_loops::claw_queue.status);
+
+ if (::std::abs(goal_angle - angle) < kGoalAngleEpsilon) {
+ return true;
+ }
+ }
+}
+
+bool HorizontalCanPickupActor::WaitUntilNear(double angle) {
+ while (true) {
+ control_loops::claw_queue.status.FetchAnother();
+ if (ShouldCancel()) return false;
+ const double current_angle = control_loops::claw_queue.status->angle;
+ LOG_STRUCT(DEBUG, "Got claw status", *control_loops::claw_queue.status);
+
+ if (::std::abs(current_angle - angle) < kAngleEpsilon) {
+ return true;
+ }
+ }
+}
+void HorizontalCanPickupActor::MoveArm(double angle, double intake_power) {
+ MoveArm(angle, intake_power, kClawPickup);
+}
+
+void HorizontalCanPickupActor::MoveArm(double angle, double intake_power,
+ const ProfileParams profile_params) {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = angle;
+ message->max_velocity = profile_params.velocity;
+ message->max_acceleration = profile_params.acceleration;
+ message->angular_velocity = 0.0;
+ message->intake = intake_power;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+}
+
+bool HorizontalCanPickupActor::RunAction(
+ const HorizontalCanPickupParams ¶ms) {
+ // Go around the can.
+ if (!StartFridgeProfile(params.elevator_height, 0.0, kFastElevatorMove,
+ kFastArmMove, false, false, true)) {
+ return true;
+ }
+
+ control_loops::claw_queue.status.FetchAnother();
+
+ MoveArm(control_loops::claw_queue.status->angle, params.spit_power);
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.spit_time))) {
+ return true;
+ }
+
+ MoveArm(params.pickup_angle, 0.0, kClawInitialLift);
+
+ if (!WaitUntilNear(params.pickup_angle)) {
+ return true;
+ }
+
+ MoveArm(params.pickup_angle, params.suck_power);
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.suck_time))) {
+ return true;
+ }
+
+ MoveArm(0.0, 0.0, kClawBackDown);
+
+ if (!WaitUntilGoalNear(0.0)) {
+ return true;
+ }
+
+ MoveArm(0.0, params.claw_settle_power);
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.claw_settle_time))) {
+ return true;
+ }
+
+ while (true) {
+ ProfileStatus status =
+ IterateProfile(params.elevator_height, 0.0, kFastElevatorMove,
+ kFastArmMove, false, false, true);
+ if (status == DONE) {
+ break;
+ } else if (status == CANCELED) {
+ return true;
+ }
+ }
+
+ MoveArm(params.claw_full_lift_angle, 0.0);
+
+ if (!WaitUntilNear(params.claw_full_lift_angle)) {
+ return true;
+ }
+
+ DoFridgeProfile(params.elevator_height, 0.0, kElevatorMove, kArmMove, false,
+ true, true);
+ if (ShouldCancel()) return true;
+
+ MoveArm(params.claw_end_angle, 7.0);
+
+ if (!WaitUntilNear(params.claw_end_angle)) {
+ return true;
+ }
+ MoveArm(params.claw_end_angle, 0.0);
+
+ if (ShouldCancel()) return true;
+
+ DoFridgeProfile(params.elevator_end_height, params.arm_end_angle,
+ kElevatorMove, kArmMove, false, true, true);
+
+ return true;
+}
+
+::std::unique_ptr<HorizontalCanPickupAction> MakeHorizontalCanPickupAction(
+ const HorizontalCanPickupParams ¶ms) {
+ return ::std::unique_ptr<HorizontalCanPickupAction>(
+ new HorizontalCanPickupAction(
+ &::frc971::actors::horizontal_can_pickup_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/horizontal_can_pickup_actor.h b/y2015/actors/horizontal_can_pickup_actor.h
new file mode 100644
index 0000000..5510178
--- /dev/null
+++ b/y2015/actors/horizontal_can_pickup_actor.h
@@ -0,0 +1,45 @@
+#ifndef Y2015_ACTORS_HORIZONTAL_CAN_PICKUP_ACTOR_H_
+#define Y2015_ACTORS_HORIZONTAL_CAN_PICKUP_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/horizontal_can_pickup_action.q.h"
+#include "y2015/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class HorizontalCanPickupActor
+ : public FridgeActorBase<HorizontalCanPickupActionQueueGroup> {
+ public:
+ explicit HorizontalCanPickupActor(
+ HorizontalCanPickupActionQueueGroup *queues);
+
+ bool RunAction(const HorizontalCanPickupParams ¶ms) override;
+
+ private:
+ // Waits until we are near the angle.
+ // Returns false if we should cancel.
+ bool WaitUntilNear(double angle);
+ bool WaitUntilGoalNear(double angle);
+
+ void MoveArm(double angle, double intake_power);
+ void MoveArm(double angle, double intake_power,
+ const ProfileParams profile_params);
+};
+
+typedef aos::common::actions::TypedAction<HorizontalCanPickupActionQueueGroup>
+ HorizontalCanPickupAction;
+
+// Makes a new HorizontalCanPickupActor action.
+::std::unique_ptr<HorizontalCanPickupAction> MakeHorizontalCanPickupAction(
+ const HorizontalCanPickupParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // Y2015_ACTORS_HORIZONTAL_CAN_PICKUP_ACTOR_H_
diff --git a/y2015/actors/horizontal_can_pickup_actor_main.cc b/y2015/actors/horizontal_can_pickup_actor_main.cc
new file mode 100644
index 0000000..67f986c
--- /dev/null
+++ b/y2015/actors/horizontal_can_pickup_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/horizontal_can_pickup_action.q.h"
+#include "y2015/actors/horizontal_can_pickup_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::HorizontalCanPickupActor horizontal_can_pickup(
+ &::frc971::actors::horizontal_can_pickup_action);
+ horizontal_can_pickup.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/actors/lift_action.q b/y2015/actors/lift_action.q
new file mode 100644
index 0000000..37fba8d
--- /dev/null
+++ b/y2015/actors/lift_action.q
@@ -0,0 +1,18 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+import "y2015/actors/lift_action_params.q";
+
+queue_group LiftActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ LiftParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group LiftActionQueueGroup lift_action;
diff --git a/y2015/actors/lift_action_params.q b/y2015/actors/lift_action_params.q
new file mode 100644
index 0000000..b31982b
--- /dev/null
+++ b/y2015/actors/lift_action_params.q
@@ -0,0 +1,19 @@
+package frc971.actors;
+
+// Parameters to send with start.
+struct LiftParams {
+ // Lift height
+ double lift_height;
+ // Arm goal.
+ double lift_arm;
+
+ // If true, do the second lift.
+ bool second_lift;
+ // Arm goal.
+ double intermediate_lift_height;
+
+ // True to move the claw in the middle of the lift.
+ bool pack_claw;
+ // Iff pack_claw is true, the angle to move the claw to.
+ double pack_claw_angle;
+};
diff --git a/y2015/actors/lift_actor.cc b/y2015/actors/lift_actor.cc
new file mode 100644
index 0000000..c057eef
--- /dev/null
+++ b/y2015/actors/lift_actor.cc
@@ -0,0 +1,92 @@
+#include <math.h>
+
+#include "aos/common/time.h"
+#include "y2015/actors/lift_actor.h"
+#include "y2015/constants.h"
+#include "y2015/actors/fridge_profile_lib.h"
+#include "y2015/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr ProfileParams kArmMove{0.6, 1.0};
+constexpr ProfileParams kElevatorMove{0.9, 3.0};
+constexpr ProfileParams kElevatorFixMove{0.9, 2.0};
+} // namespace
+
+LiftActor::LiftActor(LiftActionQueueGroup *queues)
+ : FridgeActorBase<LiftActionQueueGroup>(queues) {}
+
+bool LiftActor::RunAction(const LiftParams ¶ms) {
+ control_loops::fridge_queue.status.FetchLatest();
+ if (!control_loops::fridge_queue.status.get()) {
+ return false;
+ }
+
+ double goal_height = params.lift_height;
+ double goal_angle = 0.0;
+
+ if (params.second_lift) {
+ DoFridgeProfile(params.intermediate_lift_height, 0.0, kElevatorFixMove,
+ kArmMove,
+ control_loops::fridge_queue.status->grabbers.top_front,
+ control_loops::fridge_queue.status->grabbers.bottom_front,
+ control_loops::fridge_queue.status->grabbers.bottom_back);
+ if (ShouldCancel()) return true;
+ }
+
+ if (!StartFridgeProfile(
+ params.lift_height, 0.0, kElevatorMove, kArmMove,
+ control_loops::fridge_queue.status->grabbers.top_front,
+ control_loops::fridge_queue.status->grabbers.bottom_front,
+ control_loops::fridge_queue.status->grabbers.bottom_back)) {
+ return true;
+ }
+
+ bool has_started_back = false;
+ while (true) {
+ if (control_loops::fridge_queue.status->goal_height > 0.1) {
+ if (!has_started_back) {
+ if (!StartFridgeProfile(
+ params.lift_height, params.lift_arm, kElevatorMove, kArmMove,
+ control_loops::fridge_queue.status->grabbers.top_front,
+ control_loops::fridge_queue.status->grabbers.bottom_front,
+ control_loops::fridge_queue.status->grabbers.bottom_back)) {
+ return true;
+ }
+ goal_angle = params.lift_arm;
+ has_started_back = true;
+ if (params.pack_claw) {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.pack_claw_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();
+ }
+ }
+ }
+
+ ProfileStatus status = IterateProfile(
+ goal_height, goal_angle, kElevatorMove, kArmMove,
+ control_loops::fridge_queue.status->grabbers.top_front,
+ control_loops::fridge_queue.status->grabbers.bottom_front,
+ control_loops::fridge_queue.status->grabbers.bottom_back);
+ if (status == DONE || status == CANCELED) {
+ return true;
+ }
+ }
+ return true;
+}
+
+::std::unique_ptr<LiftAction> MakeLiftAction(const LiftParams ¶ms) {
+ return ::std::unique_ptr<LiftAction>(
+ new LiftAction(&::frc971::actors::lift_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/lift_actor.h b/y2015/actors/lift_actor.h
new file mode 100644
index 0000000..cbd79ec
--- /dev/null
+++ b/y2015/actors/lift_actor.h
@@ -0,0 +1,31 @@
+#ifndef Y2015_ACTORS_LIFT_ACTOR_H_
+#define Y2015_ACTORS_LIFT_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/lift_action.q.h"
+#include "y2015/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class LiftActor : public FridgeActorBase<LiftActionQueueGroup> {
+ public:
+ explicit LiftActor(LiftActionQueueGroup *queues);
+
+ bool RunAction(const LiftParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<LiftActionQueueGroup> LiftAction;
+
+// Makes a new LiftActor action.
+::std::unique_ptr<LiftAction> MakeLiftAction(const LiftParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // Y2015_ACTORS_LIFT_ACTOR_H_
diff --git a/y2015/actors/lift_actor_main.cc b/y2015/actors/lift_actor_main.cc
new file mode 100644
index 0000000..ed809ef
--- /dev/null
+++ b/y2015/actors/lift_actor_main.cc
@@ -0,0 +1,15 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/lift_action.q.h"
+#include "y2015/actors/lift_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::LiftActor lift(&::frc971::actors::lift_action);
+ lift.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/actors/pickup_action.q b/y2015/actors/pickup_action.q
new file mode 100644
index 0000000..237337a
--- /dev/null
+++ b/y2015/actors/pickup_action.q
@@ -0,0 +1,33 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+// Parameters to send with start.
+struct PickupParams {
+ // Angle to pull in at the top.
+ double pickup_angle;
+ // Angle to start sucking in above.
+ double suck_angle;
+ // Angle to finish sucking at.
+ double suck_angle_finish;
+ // Angle to go to once we get to the top to finish pulling in.
+ double pickup_finish_angle;
+ // Power to pull the bin in at the top.
+ double intake_voltage;
+ // Time to pull the bin in for.
+ double intake_time;
+};
+
+queue_group PickupActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ PickupParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group PickupActionQueueGroup pickup_action;
diff --git a/y2015/actors/pickup_actor.cc b/y2015/actors/pickup_actor.cc
new file mode 100644
index 0000000..0863ba6
--- /dev/null
+++ b/y2015/actors/pickup_actor.cc
@@ -0,0 +1,149 @@
+#include "y2015/actors/pickup_actor.h"
+
+#include <cmath>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/time.h"
+#include "y2015/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr double kClawPickupVelocity = 3.00;
+constexpr double kClawPickupAcceleration = 3.5;
+constexpr double kClawMoveDownVelocity = 7.00;
+constexpr double kClawMoveDownAcceleration = 15.0;
+constexpr double kClawMoveUpVelocity = 8.0;
+constexpr double kClawMoveUpAcceleration = 25.0;
+} // namespace
+
+PickupActor::PickupActor(PickupActionQueueGroup* queues)
+ : aos::common::actions::ActorBase<PickupActionQueueGroup>(queues) {}
+
+bool PickupActor::RunAction(const PickupParams& params) {
+ constexpr double kAngleEpsilon = 0.10;
+ // Start lifting the tote.
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.pickup_angle;
+ message->max_velocity = kClawPickupVelocity;
+ message->max_acceleration = kClawPickupAcceleration;
+ message->angular_velocity = 0.0;
+ message->intake = 0.0;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+ while (true) {
+ control_loops::claw_queue.status.FetchAnother();
+ if (ShouldCancel()) return true;
+ const double current_angle = control_loops::claw_queue.status->angle;
+ LOG_STRUCT(DEBUG, "Got claw status", *control_loops::claw_queue.status);
+
+ if (current_angle > params.suck_angle ||
+ ::std::abs(current_angle - params.pickup_angle) < kAngleEpsilon) {
+ break;
+ }
+ }
+
+ // Once above params.suck_angle, start sucking while lifting.
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.pickup_angle;
+ message->max_velocity = kClawPickupVelocity;
+ message->max_acceleration = kClawPickupAcceleration;
+ message->angular_velocity = 0.0;
+ message->intake = params.intake_voltage;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+
+ while (true) {
+ control_loops::claw_queue.status.FetchAnother();
+ if (ShouldCancel()) return true;
+ const double current_angle = control_loops::claw_queue.status->angle;
+ LOG_STRUCT(DEBUG, "Got claw status", *control_loops::claw_queue.status);
+
+ if (::std::abs(current_angle - params.pickup_angle) < kAngleEpsilon) {
+ break;
+ }
+ }
+
+ // Now that we have reached the upper height, come back down while intaking.
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.suck_angle_finish;
+ message->max_velocity = kClawMoveDownVelocity;
+ message->max_acceleration = kClawMoveDownAcceleration;
+ message->angular_velocity = 0.0;
+ message->intake = params.intake_voltage;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+
+ // Pull in for params.intake_time.
+ ::aos::time::Time end_time =
+ ::aos::time::Time::Now() + aos::time::Time::InSeconds(params.intake_time);
+ while ( ::aos::time::Time::Now() <= end_time) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) return true;
+ }
+
+ // Lift the claw back up to pack the box back in.
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.pickup_finish_angle;
+ message->max_velocity = kClawMoveUpVelocity;
+ message->max_acceleration = kClawMoveUpAcceleration;
+ message->angular_velocity = 0.0;
+ message->intake = params.intake_voltage;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+
+ while (true) {
+ control_loops::claw_queue.status.FetchAnother();
+ if (ShouldCancel()) return true;
+ const double current_angle = control_loops::claw_queue.status->angle;
+ LOG_STRUCT(DEBUG, "Got claw status", *control_loops::claw_queue.status);
+
+ if (::std::abs(current_angle - params.pickup_finish_angle) <
+ kAngleEpsilon) {
+ break;
+ }
+ }
+
+ // Stop the motors...
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.pickup_finish_angle;
+ message->max_velocity = kClawMoveUpVelocity;
+ message->max_acceleration = kClawMoveUpAcceleration;
+ message->angular_velocity = 0.0;
+ message->intake = 0.0;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+
+
+ return true;
+}
+
+::std::unique_ptr<PickupAction> MakePickupAction(const PickupParams& params) {
+ return ::std::unique_ptr<PickupAction>(
+ new PickupAction(&::frc971::actors::pickup_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/pickup_actor.h b/y2015/actors/pickup_actor.h
new file mode 100644
index 0000000..1f09718
--- /dev/null
+++ b/y2015/actors/pickup_actor.h
@@ -0,0 +1,27 @@
+#ifndef Y2015_ACTORS_PICKUP_ACTOR_H_
+#define Y2015_ACTORS_PICKUP_ACTOR_H_
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/pickup_action.q.h"
+
+namespace frc971 {
+namespace actors {
+
+class PickupActor
+ : public aos::common::actions::ActorBase<PickupActionQueueGroup> {
+ public:
+ explicit PickupActor(PickupActionQueueGroup *queues);
+
+ bool RunAction(const PickupParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<PickupActionQueueGroup> PickupAction;
+
+// Makes a new PickupActor action.
+::std::unique_ptr<PickupAction> MakePickupAction(const PickupParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // Y2015_ACTORS_PICKUP_ACTOR_H_
diff --git a/y2015/actors/pickup_actor_main.cc b/y2015/actors/pickup_actor_main.cc
new file mode 100644
index 0000000..9c2c488
--- /dev/null
+++ b/y2015/actors/pickup_actor_main.cc
@@ -0,0 +1,15 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/pickup_action.q.h"
+#include "y2015/actors/pickup_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actors::PickupActor pickup(&::frc971::actors::pickup_action);
+ pickup.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/actors/score_action.q b/y2015/actors/score_action.q
new file mode 100644
index 0000000..5b14210
--- /dev/null
+++ b/y2015/actors/score_action.q
@@ -0,0 +1,34 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+// Parameters to send with start.
+struct ScoreParams {
+ // If true, move the stack first.
+ bool move_the_stack;
+ // If true, place the stack (possibly after moving it).
+ bool place_the_stack;
+
+ // TODO(Brian): Comments (by somebody who knows what these all mean).
+ double upper_move_height;
+ double begin_horizontal_move_height;
+ double horizontal_move_target;
+ double horizontal_start_lowering;
+ double place_height;
+ double home_lift_horizontal_start_position;
+ double home_return_height;
+};
+
+queue_group ScoreActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ ScoreParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group ScoreActionQueueGroup score_action;
diff --git a/y2015/actors/score_actor.cc b/y2015/actors/score_actor.cc
new file mode 100644
index 0000000..c0a398f
--- /dev/null
+++ b/y2015/actors/score_actor.cc
@@ -0,0 +1,296 @@
+#include "y2015/actors/score_actor.h"
+
+#include <cmath>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "y2015/constants.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
+
+using ::frc971::control_loops::fridge_queue;
+
+namespace frc971 {
+namespace actors {
+namespace {
+
+const double kSlowMaxXVelocity = 0.80;
+const double kSlowMaxYVelocity = 0.40;
+const double kFastMaxXVelocity = 0.80;
+const double kFastMaxYVelocity = 0.30;
+const double kReallyFastMaxXVelocity = 1.0;
+const double kReallyFastMaxYVelocity = 0.6;
+
+const double kMaxXAcceleration = 0.5;
+const double kMaxYAcceleration = 0.7;
+const double kLiftYAcceleration = 2.0;
+const double kLowerYAcceleration = 2.0;
+const double kFastMaxXAcceleration = 1.5;
+const double kFastMaxYAcceleration = 1.5;
+const double kSlowMaxXAcceleration = 0.3;
+const double kSlowMaxYAcceleration = 0.5;
+
+} // namespace
+
+ScoreActor::ScoreActor(ScoreActionQueueGroup* queues)
+ : aos::common::actions::ActorBase<ScoreActionQueueGroup>(queues),
+ kinematics_(constants::GetValues().fridge.arm_length,
+ constants::GetValues().fridge.elevator.upper_limit,
+ constants::GetValues().fridge.elevator.lower_limit,
+ constants::GetValues().fridge.arm.upper_limit,
+ constants::GetValues().fridge.arm.lower_limit) {}
+
+bool ScoreActor::RunAction(const ScoreParams& params) {
+ if (params.move_the_stack) {
+ LOG(INFO, "moving stack\n");
+ if (!MoveStackIntoPosition(params)) return false;
+ LOG(INFO, "done moving stack\n");
+ if (ShouldCancel()) return true;
+ }
+ if (params.place_the_stack) {
+ LOG(INFO, "placing stack\n");
+ if (!PlaceTheStack(params)) return false;
+ LOG(INFO, "done placing stack\n");
+ if (ShouldCancel()) return true;
+ }
+ return true;
+}
+
+bool ScoreActor::MoveStackIntoPosition(const ScoreParams& params) {
+ // Move the fridge up a little bit.
+ if (!SendGoal(0.0, params.upper_move_height, true, kSlowMaxXVelocity,
+ kSlowMaxYVelocity, kMaxXAcceleration, kLiftYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) {
+ return true;
+ }
+
+ // Move on when it is clear of the tote knobs.
+ if (CurrentGoalHeight() > params.begin_horizontal_move_height) {
+ LOG(INFO, "moving on to horizontal move\n");
+ break;
+ }
+ }
+
+ // Move the fridge out.
+ if (!SendGoal(params.horizontal_move_target,
+ params.begin_horizontal_move_height, true, kSlowMaxXVelocity,
+ kFastMaxYVelocity, kSlowMaxXAcceleration,
+ kSlowMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ bool started_lowering = false;
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) {
+ return true;
+ }
+ // Round the moving out corner and start setting down.
+ if (params.place_the_stack && !started_lowering) {
+ if (CurrentGoalX() < params.horizontal_start_lowering) {
+ if (!SendGoal(params.horizontal_move_target, params.place_height, true,
+ kSlowMaxXVelocity, kFastMaxYVelocity,
+ kSlowMaxXAcceleration, kMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+ started_lowering = true;
+ }
+ }
+
+ if (NearHorizontalGoal(params.horizontal_move_target)) {
+ LOG(INFO, "reached goal\n");
+ break;
+ }
+ }
+
+ if (ShouldCancel()) return true;
+
+ return true;
+}
+
+bool ScoreActor::PlaceTheStack(const ScoreParams& params) {
+ // Once the fridge is way out, put it on the ground.
+ if (!SendGoal(params.horizontal_move_target, params.place_height, true,
+ kFastMaxXVelocity, kFastMaxYVelocity, kMaxXAcceleration,
+ kLowerYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ while (true) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) {
+ return true;
+ }
+
+ if (NearGoal(params.horizontal_move_target, params.place_height)) {
+ break;
+ }
+ }
+
+ // Release and give the grabers a chance to get out of the way.
+ if (!SendGoal(params.horizontal_move_target, params.place_height, false,
+ kFastMaxXVelocity, kFastMaxYVelocity, kMaxXAcceleration,
+ kLowerYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+ if (!WaitOrCancel(::aos::time::Time::InSeconds(0.1))) return true;
+
+ // Go back to the home position.
+ if (!SendGoal(0.0, params.place_height, false, kReallyFastMaxXVelocity,
+ kReallyFastMaxYVelocity, kFastMaxXAcceleration,
+ kFastMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+
+ bool has_lifted = false;
+ while (true) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ if (ShouldCancel()) {
+ return true;
+ }
+
+ if (!has_lifted &&
+ CurrentGoalX() > params.home_lift_horizontal_start_position) {
+ if (!SendGoal(0.0, params.home_return_height, false,
+ kReallyFastMaxXVelocity, kReallyFastMaxYVelocity,
+ kFastMaxXAcceleration, kFastMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+ has_lifted = true;
+ }
+
+ if (NearGoal(0.0, params.home_return_height)) {
+ break;
+ }
+ }
+
+ return true;
+}
+
+double ScoreActor::CurrentHeight() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return false;
+ }
+
+ ::aos::util::ElevatorArmKinematics::KinematicResult results;
+ kinematics_.ForwardKinematic(fridge_queue.status->height,
+ fridge_queue.status->angle, 0.0, 0.0, &results);
+
+ return results.fridge_h;
+}
+
+double ScoreActor::CurrentGoalHeight() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ ::aos::util::ElevatorArmKinematics::KinematicResult results;
+ kinematics_.ForwardKinematic(fridge_queue.status->goal_height,
+ fridge_queue.status->goal_angle, 0.0, 0.0,
+ &results);
+
+ return results.fridge_h;
+}
+
+double ScoreActor::CurrentX() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return false;
+ }
+
+ return fridge_queue.status->x;
+}
+
+double ScoreActor::CurrentGoalX() {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return 0.0;
+ }
+
+ return fridge_queue.status->goal_x;
+}
+
+bool ScoreActor::SendGoal(double x, double y, bool grabbers_enabled,
+ double max_x_velocity, double max_y_velocity,
+ double max_x_acceleration,
+ double max_y_acceleration) {
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ new_fridge_goal->x = x;
+ new_fridge_goal->y = y;
+ new_fridge_goal->profiling_type = 1;
+ new_fridge_goal->angular_velocity = 0.0;
+ new_fridge_goal->velocity = 0.0;
+ new_fridge_goal->grabbers.top_front = grabbers_enabled;
+ new_fridge_goal->grabbers.top_back = grabbers_enabled;
+ new_fridge_goal->grabbers.bottom_front = grabbers_enabled;
+ new_fridge_goal->grabbers.bottom_back = grabbers_enabled;
+ new_fridge_goal->max_x_velocity = max_x_velocity;
+ new_fridge_goal->max_y_velocity = max_y_velocity;
+ new_fridge_goal->max_x_acceleration = max_x_acceleration;
+ new_fridge_goal->max_y_acceleration = max_y_acceleration;
+ LOG_STRUCT(INFO, "sending fridge goal", *new_fridge_goal);
+
+ return new_fridge_goal.Send();
+}
+
+bool ScoreActor::NearHorizontalGoal(double x) {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return false;
+ }
+
+ ::aos::util::ElevatorArmKinematics::KinematicResult results;
+ ::aos::util::ElevatorArmKinematics::KinematicResult goal_results;
+ kinematics_.ForwardKinematic(fridge_queue.status->height,
+ fridge_queue.status->angle, 0.0, 0.0, &results);
+ kinematics_.ForwardKinematic(fridge_queue.status->goal_height,
+ fridge_queue.status->goal_angle, 0.0, 0.0,
+ &goal_results);
+
+ return (::std::abs(results.fridge_x - x) < 0.020 &&
+ ::std::abs(goal_results.fridge_x - x) < 0.0001);
+}
+
+bool ScoreActor::NearGoal(double x, double y) {
+ fridge_queue.status.FetchLatest();
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Reading from fridge status queue failed.\n");
+ return false;
+ }
+
+ const auto &status = *fridge_queue.status;
+ return (::std::abs(status.x - x) < 0.020 &&
+ ::std::abs(status.y - y) < 0.020 &&
+ ::std::abs(status.goal_x - x) < 0.0001 &&
+ ::std::abs(status.goal_y - y) < 0.0001);
+}
+
+::std::unique_ptr<ScoreAction> MakeScoreAction(const ScoreParams& params) {
+ return ::std::unique_ptr<ScoreAction>(
+ new ScoreAction(&::frc971::actors::score_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/score_actor.h b/y2015/actors/score_actor.h
new file mode 100644
index 0000000..c9ed71f
--- /dev/null
+++ b/y2015/actors/score_actor.h
@@ -0,0 +1,43 @@
+#ifndef Y2015_ACTORS_SCORE_ACTOR_H_
+#define Y2015_ACTORS_SCORE_ACTOR_H_
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/util/kinematics.h"
+#include "y2015/actors/score_action.q.h"
+
+namespace frc971 {
+namespace actors {
+
+class ScoreActor
+ : public ::aos::common::actions::ActorBase<ScoreActionQueueGroup> {
+ public:
+ explicit ScoreActor(ScoreActionQueueGroup *queues);
+
+ bool RunAction(const ScoreParams ¶ms) override;
+
+ private:
+
+ ::aos::util::ElevatorArmKinematics kinematics_;
+ bool NearGoal(double x, double y);
+ bool NearHorizontalGoal(double x);
+ bool PlaceTheStack(const ScoreParams ¶ms);
+ bool MoveStackIntoPosition(const ScoreParams ¶ms);
+ bool SendGoal(double x, double y, bool grabbers_enabled,
+ double max_x_velocity, double max_y_velocity,
+ double max_x_acceleration, double max_y_acceleration);
+ double CurrentHeight();
+ double CurrentGoalHeight();
+ double CurrentX();
+ double CurrentGoalX();
+};
+
+typedef aos::common::actions::TypedAction<ScoreActionQueueGroup> ScoreAction;
+
+// Makes a new ScoreActor action.
+::std::unique_ptr<ScoreAction> MakeScoreAction(const ScoreParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif
diff --git a/y2015/actors/score_actor_main.cc b/y2015/actors/score_actor_main.cc
new file mode 100644
index 0000000..cc149e6
--- /dev/null
+++ b/y2015/actors/score_actor_main.cc
@@ -0,0 +1,15 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/score_action.q.h"
+#include "y2015/actors/score_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actors::ScoreActor score(&::frc971::actors::score_action);
+ score.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/actors/score_actor_test.cc b/y2015/actors/score_actor_test.cc
new file mode 100644
index 0000000..4791f09
--- /dev/null
+++ b/y2015/actors/score_actor_test.cc
@@ -0,0 +1,98 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/score_action.q.h"
+#include "y2015/actors/score_actor.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace actors {
+namespace testing {
+
+class ScoreActionTest : public ::testing::Test {
+ protected:
+ ScoreActionTest() {
+ frc971::actors::score_action.goal.Clear();
+ frc971::actors::score_action.status.Clear();
+ control_loops::fridge_queue.status.Clear();
+ control_loops::fridge_queue.goal.Clear();
+ }
+
+ virtual ~ScoreActionTest() {
+ frc971::actors::score_action.goal.Clear();
+ frc971::actors::score_action.status.Clear();
+ control_loops::fridge_queue.status.Clear();
+ control_loops::fridge_queue.goal.Clear();
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+};
+
+// Tests that cancel stops not only the score action, but also the underlying
+// profile action.
+TEST_F(ScoreActionTest, PlaceTheStackCancel) {
+ ScoreActor score(&frc971::actors::score_action);
+
+ frc971::actors::score_action.goal.MakeWithBuilder().run(true).Send();
+
+ // Tell it the fridge is zeroed.
+ ASSERT_TRUE(control_loops::fridge_queue.status.MakeWithBuilder()
+ .zeroed(true)
+ .angle(0.0)
+ .height(0.0)
+ .Send());
+
+ // do the action and it will post to the goal queue
+ score.WaitForActionRequest();
+
+ // the action has started, so now cancel it and it should cancel
+ // the underlying profile
+ frc971::actors::score_action.goal.MakeWithBuilder().run(false).Send();
+
+ // let the action start running, if we return from this call it has worked.
+ const ScoreParams params = {true, true, 0.14, 0.13, -0.7, -0.7, -0.10, -0.5, 0.1};
+ score.RunAction(params);
+
+ SUCCEED();
+}
+
+// Tests that cancel stops not only the score action, but also the underlying
+// profile action.
+TEST_F(ScoreActionTest, MoveStackIntoPositionCancel) {
+ ScoreActor score(&frc971::actors::score_action);
+
+ frc971::actors::score_action.goal.MakeWithBuilder().run(true).Send();
+
+ // Tell it the fridge is zeroed.
+ ASSERT_TRUE(control_loops::fridge_queue.status.MakeWithBuilder()
+ .zeroed(true)
+ .angle(0.0)
+ .height(0.0)
+ .Send());
+
+ // do the action and it will post to the goal queue
+ score.WaitForActionRequest();
+
+ // the action has started, so now cancel it and it should cancel
+ // the underlying profile
+ frc971::actors::score_action.goal.MakeWithBuilder().run(false).Send();
+
+ // let the action start running, if we return from this call it has worked.
+ const ScoreParams params = {false, true, 0.14, 0.13, -0.7, -0.7, -0.10, -0.5, 0.1};
+ score.RunAction(params);
+
+ SUCCEED();
+}
+
+} // namespace testing
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/stack_action.q b/y2015/actors/stack_action.q
new file mode 100644
index 0000000..b4389b8
--- /dev/null
+++ b/y2015/actors/stack_action.q
@@ -0,0 +1,18 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+import "y2015/actors/stack_action_params.q";
+
+queue_group StackActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ StackParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group StackActionQueueGroup stack_action;
diff --git a/y2015/actors/stack_action_params.q b/y2015/actors/stack_action_params.q
new file mode 100644
index 0000000..3958f7d
--- /dev/null
+++ b/y2015/actors/stack_action_params.q
@@ -0,0 +1,17 @@
+package frc971.actors;
+
+// Parameters to send with start.
+struct StackParams {
+ // If true, don't grab the lower tote after lowering.
+ bool only_place;
+
+ // The angle to move the arm to while lowering it.
+ double arm_clearance;
+
+ double claw_out_angle;
+ // The height just above the box to move before lowering.
+ double over_box_before_place_height;
+
+ // Bottom position.
+ double bottom;
+};
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
diff --git a/y2015/actors/stack_actor.h b/y2015/actors/stack_actor.h
new file mode 100644
index 0000000..5f2f7fb
--- /dev/null
+++ b/y2015/actors/stack_actor.h
@@ -0,0 +1,31 @@
+#ifndef Y2015_ACTORS_STACK_ACTOR_H_
+#define Y2015_ACTORS_STACK_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/stack_action.q.h"
+#include "y2015/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class StackActor : public FridgeActorBase<StackActionQueueGroup> {
+ public:
+ explicit StackActor(StackActionQueueGroup *queues);
+
+ bool RunAction(const StackParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<StackActionQueueGroup> StackAction;
+
+// Makes a new stackActor action.
+::std::unique_ptr<StackAction> MakeStackAction(const StackParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif
diff --git a/y2015/actors/stack_actor_main.cc b/y2015/actors/stack_actor_main.cc
new file mode 100644
index 0000000..f65733c
--- /dev/null
+++ b/y2015/actors/stack_actor_main.cc
@@ -0,0 +1,15 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/stack_action.q.h"
+#include "y2015/actors/stack_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::StackActor stack(&::frc971::actors::stack_action);
+ stack.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/actors/stack_actor_test.cc b/y2015/actors/stack_actor_test.cc
new file mode 100644
index 0000000..e1b1856
--- /dev/null
+++ b/y2015/actors/stack_actor_test.cc
@@ -0,0 +1,72 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/stack_action.q.h"
+#include "y2015/actors/stack_actor.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
+
+#include "aos/common/controls/control_loop_test.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace actors {
+namespace testing {
+
+class StackActionTest : public ::testing::Test {
+ protected:
+ StackActionTest() {
+ frc971::actors::stack_action.goal.Clear();
+ frc971::actors::stack_action.status.Clear();
+ control_loops::fridge_queue.status.Clear();
+ control_loops::fridge_queue.goal.Clear();
+ }
+
+ virtual ~StackActionTest() {
+ frc971::actors::stack_action.goal.Clear();
+ frc971::actors::stack_action.status.Clear();
+ control_loops::fridge_queue.status.Clear();
+ control_loops::fridge_queue.goal.Clear();
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+};
+
+// Tests that cancel stops not only the stack action, but the underlying profile
+// action.
+TEST_F(StackActionTest, StackCancel) {
+ StackActor stack(&frc971::actors::stack_action);
+
+ frc971::actors::stack_action.goal.MakeWithBuilder().run(true).Send();
+
+ // tell it the fridge is zeroed
+ control_loops::fridge_queue.status.MakeWithBuilder()
+ .zeroed(true)
+ .angle(0.0)
+ .height(0.0)
+ .Send();
+
+ // do the action and it will post to the goal queue
+ stack.WaitForActionRequest();
+
+ // the action has started, so now cancel it and it should cancel
+ // the underlying profile
+ frc971::actors::stack_action.goal.MakeWithBuilder().run(false).Send();
+
+ // let the action start running, if we return from this call it has worked.
+ StackParams s;
+ stack.RunAction(s);
+
+ SUCCEED();
+}
+
+} // namespace testing
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/stack_and_hold_action.q b/y2015/actors/stack_and_hold_action.q
new file mode 100644
index 0000000..42e98ed
--- /dev/null
+++ b/y2015/actors/stack_and_hold_action.q
@@ -0,0 +1,42 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+import "y2015/actors/stack_action_params.q";
+
+// Parameters to send with start.
+struct StackAndHoldParams {
+ // If true, there is no tote on the tray, and we should place instead.
+ bool place_not_stack;
+
+ double claw_out_angle;
+ // The height just above the box to move before lowering.
+ double over_box_before_place_height;
+
+ // Bottom position.
+ double bottom;
+
+ // If we are placing, clamp the stack with the claw.
+ double claw_clamp_angle;
+
+ // Amount to wait to release.
+ double clamp_pause_time;
+
+ // The value to move the arm forwards to clear the stack when lifting.
+ double arm_clearance;
+ // Hold height
+ double hold_height;
+};
+
+queue_group StackAndHoldActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ StackAndHoldParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group StackAndHoldActionQueueGroup stack_and_hold_action;
diff --git a/y2015/actors/stack_and_hold_actor.cc b/y2015/actors/stack_and_hold_actor.cc
new file mode 100644
index 0000000..ae1eeda
--- /dev/null
+++ b/y2015/actors/stack_and_hold_actor.cc
@@ -0,0 +1,132 @@
+#include "y2015/actors/stack_and_hold_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"
+#include "y2015/actors/stack_actor.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+constexpr ProfileParams kReallySlowArmMove{0.1, 1.0};
+constexpr ProfileParams kReallySlowElevatorMove{0.10, 1.0};
+
+constexpr ProfileParams kFastArmMove{0.8, 4.0};
+constexpr ProfileParams kFastElevatorMove{1.2, 4.0};
+} // namespace
+
+StackAndHoldActor::StackAndHoldActor(StackAndHoldActionQueueGroup *queues)
+ : FridgeActorBase<StackAndHoldActionQueueGroup>(queues) {}
+
+bool StackAndHoldActor::RunAction(const StackAndHoldParams ¶ms) {
+ // TODO(ben)): this action is no longer used (source Cameron) and my be broken
+ // by the stack action having the grabbers closed at the end for the fangs. So
+ // here I am disabling it until further information is provided.
+ if (params.place_not_stack) {
+ // Move the arm out of the way.
+ {
+ 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();
+ }
+ }
+
+ // Get close, but keep the arm forwards
+ DoFridgeProfile(params.bottom + 0.04, -0.05, kFastArmMove,
+ kFastElevatorMove, true);
+
+ // Lower and pull back.
+ if (ShouldCancel()) return true;
+ DoFridgeProfile(params.bottom, 0.0, kReallySlowArmMove,
+ kReallySlowElevatorMove, true, true, false);
+
+ // Release.
+ if (ShouldCancel()) return true;
+ DoFridgeProfile(params.bottom, 0.0, kReallySlowArmMove,
+ kReallySlowElevatorMove, false);
+ } else {
+ StackParams stack_params;
+ stack_params.only_place = true;
+ stack_params.arm_clearance = params.arm_clearance;
+ stack_params.claw_out_angle = params.claw_out_angle;
+ stack_params.over_box_before_place_height =
+ params.over_box_before_place_height;
+ stack_params.bottom = params.bottom;
+ ::std::unique_ptr<StackAction> stack_action =
+ MakeStackAction(stack_params);
+ stack_action->Start();
+ while (stack_action->Running()) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ 2500);
+
+ if (ShouldCancel()) {
+ stack_action->Cancel();
+ LOG(WARNING, "Cancelling fridge and claw.\n");
+ return true;
+ }
+ }
+ }
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.clamp_pause_time))) {
+ return true;
+ }
+
+ // Go up.
+ DoFridgeProfile(params.hold_height, params.arm_clearance, kFastArmMove,
+ kFastElevatorMove, false);
+
+ if (ShouldCancel()) return true;
+
+ if (params.place_not_stack) {
+ // Clamp the stack with the claw.
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.claw_clamp_angle;
+ message->angular_velocity = 0.0;
+ message->intake = 0.0;
+ message->rollers_closed = true;
+ message->max_velocity = 6.0;
+ message->max_acceleration = 6.0;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+ // Move back
+ DoFridgeProfile(params.hold_height, 0.0, kFastArmMove, kFastElevatorMove,
+ false);
+ if (ShouldCancel()) return true;
+ // Grab
+ DoFridgeProfile(params.hold_height, 0.0, kFastArmMove, kFastElevatorMove,
+ true);
+ if (ShouldCancel()) return true;
+
+ return true;
+}
+
+::std::unique_ptr<StackAndHoldAction> MakeStackAndHoldAction(
+ const StackAndHoldParams ¶ms) {
+ return ::std::unique_ptr<StackAndHoldAction>(
+ new StackAndHoldAction(&::frc971::actors::stack_and_hold_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/stack_and_hold_actor.h b/y2015/actors/stack_and_hold_actor.h
new file mode 100644
index 0000000..b3bf2c5
--- /dev/null
+++ b/y2015/actors/stack_and_hold_actor.h
@@ -0,0 +1,33 @@
+#ifndef Y2015_ACTORS_STACK_AND_HOLD_ACTOR_H_
+#define Y2015_ACTORS_STACK_AND_HOLD_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/stack_and_hold_action.q.h"
+#include "y2015/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class StackAndHoldActor : public FridgeActorBase<StackAndHoldActionQueueGroup> {
+ public:
+ explicit StackAndHoldActor(StackAndHoldActionQueueGroup *queues);
+
+ bool RunAction(const StackAndHoldParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<StackAndHoldActionQueueGroup>
+ StackAndHoldAction;
+
+// Makes a new stackActor action.
+::std::unique_ptr<StackAndHoldAction> MakeStackAndHoldAction(
+ const StackAndHoldParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // Y2015_ACTORS_STACK_AND_HOLD_ACTOR_H_
diff --git a/y2015/actors/stack_and_hold_actor_main.cc b/y2015/actors/stack_and_hold_actor_main.cc
new file mode 100644
index 0000000..3d5d1c1
--- /dev/null
+++ b/y2015/actors/stack_and_hold_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/stack_and_hold_action.q.h"
+#include "y2015/actors/stack_and_hold_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::StackAndHoldActor stack_and_hold(
+ &::frc971::actors::stack_and_hold_action);
+ stack_and_hold.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2015/actors/stack_and_lift_action.q b/y2015/actors/stack_and_lift_action.q
new file mode 100644
index 0000000..0ec971b
--- /dev/null
+++ b/y2015/actors/stack_and_lift_action.q
@@ -0,0 +1,34 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+import "y2015/actors/stack_action_params.q";
+import "y2015/actors/lift_action_params.q";
+
+// Parameters to send with start.
+struct StackAndLiftParams {
+ // Stack parameters
+ StackParams stack_params;
+ // True if the fridge should be clamped while lifting.
+ bool grab_after_stack;
+ // The time after clamping to pause.
+ double clamp_pause_time;
+ // Lift parameters
+ LiftParams lift_params;
+ // True if the fridge should be clamped when done.
+ bool grab_after_lift;
+};
+
+queue_group StackAndLiftActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ StackAndLiftParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group StackAndLiftActionQueueGroup stack_and_lift_action;
diff --git a/y2015/actors/stack_and_lift_actor.cc b/y2015/actors/stack_and_lift_actor.cc
new file mode 100644
index 0000000..32f7417
--- /dev/null
+++ b/y2015/actors/stack_and_lift_actor.cc
@@ -0,0 +1,115 @@
+#include "y2015/actors/stack_and_lift_actor.h"
+
+#include <math.h>
+
+#include "aos/common/controls/control_loop.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"
+#include "y2015/actors/stack_actor.h"
+#include "y2015/actors/lift_actor.h"
+
+namespace frc971 {
+namespace actors {
+
+StackAndLiftActor::StackAndLiftActor(StackAndLiftActionQueueGroup *queues)
+ : FridgeActorBase<StackAndLiftActionQueueGroup>(queues) {}
+
+bool StackAndLiftActor::RunAction(const StackAndLiftParams ¶ms) {
+ control_loops::claw_queue.goal.FetchLatest();
+ double claw_goal_start;
+ bool have_claw_goal_start;
+ if (control_loops::claw_queue.goal.get()) {
+ claw_goal_start = control_loops::claw_queue.goal->angle;
+ have_claw_goal_start = true;
+ } else {
+ claw_goal_start = 0;
+ have_claw_goal_start = false;
+ }
+
+ {
+ StackParams stack_params = params.stack_params;
+ stack_params.only_place = false;
+ ::std::unique_ptr<StackAction> stack_action =
+ MakeStackAction(stack_params);
+ stack_action->Start();
+ while (stack_action->Running()) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ 2500);
+
+ if (ShouldCancel()) {
+ stack_action->Cancel();
+ LOG(WARNING, "Cancelling fridge and claw.\n");
+ return true;
+ }
+ }
+ }
+
+ {
+ control_loops::fridge_queue.goal.FetchLatest();
+ if (!control_loops::fridge_queue.goal.get()) {
+ return false;
+ }
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ *new_fridge_goal = *control_loops::fridge_queue.goal;
+ new_fridge_goal->grabbers.top_front = params.grab_after_stack;
+ new_fridge_goal->grabbers.top_back = params.grab_after_stack;
+ new_fridge_goal->grabbers.bottom_front = params.grab_after_stack;
+ new_fridge_goal->grabbers.bottom_back = params.grab_after_stack;
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge close.\n");
+ return false;
+ }
+ }
+
+ if (!WaitOrCancel(aos::time::Time::InSeconds(params.clamp_pause_time))) {
+ return true;
+ }
+
+ {
+ auto lift_params = params.lift_params;
+ lift_params.pack_claw = have_claw_goal_start;
+ lift_params.pack_claw_angle = claw_goal_start;
+ ::std::unique_ptr<LiftAction> lift_action = MakeLiftAction(lift_params);
+ lift_action->Start();
+ while (lift_action->Running()) {
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ 2500);
+
+ if (ShouldCancel()) {
+ lift_action->Cancel();
+ LOG(WARNING, "Cancelling fridge and claw.\n");
+ return true;
+ }
+ }
+ }
+
+ {
+ control_loops::fridge_queue.goal.FetchLatest();
+ if (!control_loops::fridge_queue.goal.get()) {
+ return false;
+ }
+ auto new_fridge_goal = control_loops::fridge_queue.goal.MakeMessage();
+ *new_fridge_goal = *control_loops::fridge_queue.goal;
+ new_fridge_goal->grabbers.top_front = params.grab_after_lift;
+ new_fridge_goal->grabbers.top_back = params.grab_after_lift;
+ new_fridge_goal->grabbers.bottom_front = params.grab_after_lift;
+ new_fridge_goal->grabbers.bottom_back = params.grab_after_lift;
+ if (!new_fridge_goal.Send()) {
+ LOG(ERROR, "Failed to send fridge close.\n");
+ return false;
+ }
+ }
+ return true;
+}
+
+::std::unique_ptr<StackAndLiftAction> MakeStackAndLiftAction(
+ const StackAndLiftParams ¶ms) {
+ return ::std::unique_ptr<StackAndLiftAction>(
+ new StackAndLiftAction(&::frc971::actors::stack_and_lift_action, params));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/y2015/actors/stack_and_lift_actor.h b/y2015/actors/stack_and_lift_actor.h
new file mode 100644
index 0000000..1d5fe29
--- /dev/null
+++ b/y2015/actors/stack_and_lift_actor.h
@@ -0,0 +1,33 @@
+#ifndef Y2015_ACTORS_STACK_AND_LIFT_ACTOR_H_
+#define Y2015_ACTORS_STACK_AND_LIFT_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "y2015/actors/stack_and_lift_action.q.h"
+#include "y2015/actors/fridge_profile_lib.h"
+
+namespace frc971 {
+namespace actors {
+
+class StackAndLiftActor : public FridgeActorBase<StackAndLiftActionQueueGroup> {
+ public:
+ explicit StackAndLiftActor(StackAndLiftActionQueueGroup *queues);
+
+ bool RunAction(const StackAndLiftParams ¶ms) override;
+};
+
+typedef aos::common::actions::TypedAction<StackAndLiftActionQueueGroup>
+ StackAndLiftAction;
+
+// Makes a new stackActor action.
+::std::unique_ptr<StackAndLiftAction> MakeStackAndLiftAction(
+ const StackAndLiftParams ¶ms);
+
+} // namespace actors
+} // namespace frc971
+
+#endif // Y2015_ACTORS_STACK_AND_LIFT_ACTOR_H_
diff --git a/y2015/actors/stack_and_lift_actor_main.cc b/y2015/actors/stack_and_lift_actor_main.cc
new file mode 100644
index 0000000..fe9f4b5
--- /dev/null
+++ b/y2015/actors/stack_and_lift_actor_main.cc
@@ -0,0 +1,16 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2015/actors/stack_and_lift_action.q.h"
+#include "y2015/actors/stack_and_lift_actor.h"
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init();
+
+ ::frc971::actors::StackAndLiftActor stack_and_lift(
+ &::frc971::actors::stack_and_lift_action);
+ stack_and_lift.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}