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/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