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;
+}
diff --git a/frc971/constants.cc b/frc971/constants.cc
index a8898a8..ac89391 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -43,6 +43,8 @@
2.95, 0.2, 0.7};
const ShifterHallEffect kPracticeLeftDriveShifter{2.95, 4.2, 3.95,
3.0, 0.2, 0.7};
+// TODO(danielp): Actually measure one of them...
+const double kToteHeight = 0.5;
// Set by Daniel on 2/13/15.
// Distance from the center of the left wheel to the center of the right wheel.
@@ -58,7 +60,6 @@
(2.0 * M_PI);
const double kArmZeroingHeight = 0.2;
-const double kElevatorNormalHeight = 0.1;
const double kMaxAllowedLeftRightArmDifference = 0.04; // radians
const double kMaxAllowedLeftRightElevatorDifference = 0.01; // meters
@@ -100,6 +101,7 @@
kElevatorGearboxOutputRadianDistance,
kClawEncoderRatio,
kClawPotRatio,
+ kToteHeight,
kLowGearRatio,
kHighGearRatio,
kCompLeftDriveShifter,
@@ -150,7 +152,6 @@
0.0,
kArmZeroingHeight,
- kElevatorNormalHeight,
},
// End "sensor" values.
@@ -168,6 +169,7 @@
kElevatorGearboxOutputRadianDistance,
kClawEncoderRatio,
kClawPotRatio,
+ kToteHeight,
kLowGearRatio,
kHighGearRatio,
kCompLeftDriveShifter,
@@ -219,7 +221,6 @@
3.506927 - 0.170017 - -0.147970,
kArmZeroingHeight,
- kElevatorNormalHeight,
},
// End "sensor" values.
@@ -237,6 +238,7 @@
kElevatorGearboxOutputRadianDistance,
kClawEncoderRatio,
kClawPotRatio,
+ kToteHeight,
kLowGearRatio,
kHighGearRatio,
kPracticeLeftDriveShifter,
@@ -289,7 +291,6 @@
3.5263507647058816 - 0.018921 + 0.006545,
kArmZeroingHeight,
- kElevatorNormalHeight,
},
// TODO(sensors): End "sensor" values.
diff --git a/frc971/constants.h b/frc971/constants.h
index ac6f315..18e85ae 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -36,6 +36,9 @@
// The ratio from the pot shaft to the claw joint.
double claw_pot_ratio;
+ // How tall a tote is in meters.
+ double tote_height;
+
// The gear ratios from motor shafts to the drivetrain wheels for high and low
// gear.
double low_gear_ratio;
@@ -105,8 +108,6 @@
// How high the elevator has to be before we start zeroing the arm.
double arm_zeroing_height;
- // The height at which the fridge should hang out by default.
- double normal_height;
};
Fridge fridge;
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 699d772..293fa21 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -22,6 +22,7 @@
'../actors/actors.gyp:drivetrain_action',
'../actors/actors.gyp:fridge_profile_action',
'../actors/actors.gyp:score_action',
+ '../actors/actors.gyp:stack_action',
'../actors/actors.gyp:fridge_profile_action_test',
],
'copies': [