Add stack action.
This one's pretty simple...
I also removed the elevator_normal_height constant and replaced
it with a constant for how tall a tote is. Upon further
consideration, this made more sense and seemed more useful.
Change-Id: I4a7247b9e563b1c49cf853b7b05371be172aa500
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 77ffe8f..0cd1c9b 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -167,5 +167,51 @@
'score_action_lib',
],
},
+ {
+ 'target_name': 'stack_action_queue',
+ 'type': 'static_library',
+ 'sources': ['stack_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/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': 'stack_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'stack_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_action_lib',
+ 'stack_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ 'export_dependent_settings': [
+ '<(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',
+ ],
+ },
],
}
diff --git a/frc971/actors/score_actor.cc b/frc971/actors/score_actor.cc
index 44d829b..25edbff 100644
--- a/frc971/actors/score_actor.cc
+++ b/frc971/actors/score_actor.cc
@@ -55,9 +55,10 @@
DoProfile(values.fridge.arm_zeroing_height, M_PI / 2.0, true);
// Now move them both together.
DoProfile(params.height, M_PI, true);
- // Release the totes and retract.
+ // Release the totes.
DoProfile(values.fridge.arm_zeroing_height, M_PI / 2.0, false);
- DoProfile(values.fridge.normal_height, M_PI / 2.0, false);
+ // Retract. Move back to our lowered position.
+ DoProfile(values.fridge.elevator.lower_limit, M_PI / 2.0, false);
return true;
}
diff --git a/frc971/actors/stack_action.q b/frc971/actors/stack_action.q
new file mode 100644
index 0000000..6ef4fd8
--- /dev/null
+++ b/frc971/actors/stack_action.q
@@ -0,0 +1,17 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+queue_group StackActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ uint32_t params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group StackActionQueueGroup stack_action;
diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
new file mode 100644
index 0000000..ff9c739
--- /dev/null
+++ b/frc971/actors/stack_actor.cc
@@ -0,0 +1,67 @@
+#include <math.h>
+
+#include "frc971/actors/stack_actor.h"
+#include "frc971/actors/fridge_profile_actor.h"
+#include "frc971/constants.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+
+// TODO(danielp): Real numbers!
+constexpr double kElevatorMaxVelocity = 0.5;
+constexpr double kArmMaxVelocity = 0.5;
+constexpr double kElevatorMaxAccel = 0.25;
+constexpr double kArmMaxAccel = 0.25;
+
+} // namespace
+
+StackActor::StackActor(StackActionQueueGroup* queues)
+ : aos::common::actions::ActorBase<StackActionQueueGroup>(queues) {}
+
+namespace {
+
+void DoProfile(double height, bool grabbers) {
+ FridgeProfileParams params;
+
+ params.elevator_height = height;
+ params.elevator_max_velocity = kElevatorMaxVelocity;
+ params.elevator_max_acceleration = kElevatorMaxAccel;
+
+ params.arm_angle = M_PI / 2.0;
+ params.arm_max_velocity = kArmMaxVelocity;
+ params.arm_max_acceleration = kArmMaxAccel;
+
+ params.top_front_grabber = grabbers;
+ params.top_back_grabber = grabbers;
+ params.bottom_front_grabber = grabbers;
+ params.bottom_back_grabber = grabbers;
+
+ ::std::unique_ptr<FridgeAction> profile = MakeFridgeProfileAction(params);
+ profile->Start();
+ profile->WaitUntilDone();
+}
+
+} // namespace
+
+bool StackActor::RunAction(const uint32_t&) {
+ const auto& values = constants::GetValues();
+ const double bottom = values.fridge.elevator.lower_limit;
+
+ // Set the current stack down on top of the bottom box.
+ DoProfile(bottom + values.tote_height, true);
+ // Move down to enclose bottom box.
+ DoProfile(bottom, false);
+ // Clamp.
+ DoProfile(bottom, true);
+
+ return true;
+}
+
+::std::unique_ptr<StackAction> MakeStackAction() {
+ return ::std::unique_ptr<StackAction>(
+ new StackAction(&::frc971::actors::stack_action, 0));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/frc971/actors/stack_actor.h b/frc971/actors/stack_actor.h
new file mode 100644
index 0000000..2c37770
--- /dev/null
+++ b/frc971/actors/stack_actor.h
@@ -0,0 +1,31 @@
+#ifndef FRC971_ACTORS_STACK_ACTOR_H_
+#define FRC971_ACTORS_STACK_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/actors/stack_action.q.h"
+
+namespace frc971 {
+namespace actors {
+
+class StackActor
+ : public aos::common::actions::ActorBase<StackActionQueueGroup> {
+ public:
+ explicit StackActor(StackActionQueueGroup *queues);
+
+ bool RunAction(const uint32_t&) override;
+};
+
+typedef aos::common::actions::TypedAction<StackActionQueueGroup> StackAction;
+
+// Makes a new stackActor action.
+::std::unique_ptr<StackAction> MakeStackAction();
+
+} // namespace actors
+} // namespace frc971
+
+#endif
diff --git a/frc971/actors/stack_actor_main.cc b/frc971/actors/stack_actor_main.cc
new file mode 100644
index 0000000..9a65352
--- /dev/null
+++ b/frc971/actors/stack_actor_main.cc
@@ -0,0 +1,15 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actors/stack_action.q.h"
+#include "frc971/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;
+}