Added can pickup action.
Change-Id: Ibc372b0cbec08bfd83f12751d9b5e3a050f27813
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 30bb318..2c2f212 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -51,7 +51,6 @@
'drivetrain_action_lib',
],
},
-
{
'target_name': 'fridge_profile_action_queue',
'type': 'static_library',
@@ -236,6 +235,55 @@
],
},
{
+ 'target_name': 'can_pickup_action_queue',
+ 'type': 'static_library',
+ 'sources': ['can_pickup_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': 'can_pickup_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'can_pickup_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_action_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)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(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': 'stack_action_queue',
'type': 'static_library',
'sources': ['stack_action.q'],
diff --git a/frc971/actors/can_pickup_action.q b/frc971/actors/can_pickup_action.q
new file mode 100644
index 0000000..95e0b3f
--- /dev/null
+++ b/frc971/actors/can_pickup_action.q
@@ -0,0 +1,34 @@
+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 {
+ // Angle to move the claw to when picking up.
+ double pickup_angle;
+ // Height to move the elevator to when picking up.
+ double pickup_height;
+ // Height to move the elevator to to lift the can out of the claw.
+ double lift_height;
+
+ // 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/frc971/actors/can_pickup_actor.cc b/frc971/actors/can_pickup_actor.cc
new file mode 100644
index 0000000..7ee6d4f
--- /dev/null
+++ b/frc971/actors/can_pickup_actor.cc
@@ -0,0 +1,84 @@
+#include <math.h>
+
+#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+
+#include "frc971/actors/can_pickup_actor.h"
+#include "frc971/actors/fridge_profile_actor.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+
+static constexpr double kArmVelocity = 1.00;
+static constexpr double kArmAcceleration = 1.6;
+static constexpr double kElevatorVelocity = 0.6;
+static constexpr double kElevatorAcceleration = 2.2;
+
+} // namespace
+
+// Elevator, arm
+// Go to 0.3, 0.0
+// Go to 0.35, -1.1
+
+CanPickupActor::CanPickupActor(CanPickupActionQueueGroup *queues)
+ : aos::common::actions::ActorBase<CanPickupActionQueueGroup>(queues) {}
+
+void CanPickupActor::DoProfile(double height, double angle, bool grabbers) {
+ FridgeProfileParams params;
+
+ params.elevator_height = height;
+ params.elevator_max_velocity = kElevatorVelocity;
+ params.elevator_max_acceleration = kElevatorAcceleration;
+
+ params.arm_angle = angle;
+ params.arm_max_velocity = kArmVelocity;
+ params.arm_max_acceleration = kArmAcceleration;
+
+ 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();
+ while (!profile->CheckIteration()) {
+ // wait until next Xms tick
+ ::aos::time::PhasedLoopXMS(5, 2500);
+ if (ShouldCancel()) {
+ profile->Cancel();
+ return;
+ }
+ }
+}
+
+bool CanPickupActor::RunAction(const CanPickupParams ¶ms) {
+ // Go around the can.
+ DoProfile(params.pickup_height, params.pickup_angle, false);
+ if (ShouldCancel()) return true;
+
+ // Lift and grab.
+ DoProfile(params.lift_height, params.pickup_angle, true);
+ if (ShouldCancel()) return true;
+
+ // Pull it back in.
+ DoProfile(params.lift_height, params.end_angle, true);
+ if (ShouldCancel()) return true;
+
+ // Pull it back in.
+ DoProfile(params.end_height, params.end_angle, 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/frc971/actors/can_pickup_actor.h b/frc971/actors/can_pickup_actor.h
new file mode 100644
index 0000000..407de93
--- /dev/null
+++ b/frc971/actors/can_pickup_actor.h
@@ -0,0 +1,35 @@
+#ifndef FRC971_ACTORS_CAN_PICKUP_ACTOR_H_
+#define FRC971_ACTORS_CAN_PICKUP_ACTOR_H_
+
+#include <stdint.h>
+
+#include <memory>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/actors/can_pickup_action.q.h"
+
+namespace frc971 {
+namespace actors {
+
+class CanPickupActor
+ : public aos::common::actions::ActorBase<CanPickupActionQueueGroup> {
+ public:
+ explicit CanPickupActor(CanPickupActionQueueGroup *queues);
+
+ void DoProfile(double height, double angle, bool grabbers);
+
+ bool RunAction(const CanPickupParams ¶ms) override;
+};
+
+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/frc971/actors/can_pickup_actor_main.cc b/frc971/actors/can_pickup_actor_main.cc
new file mode 100644
index 0000000..e784d94
--- /dev/null
+++ b/frc971/actors/can_pickup_actor_main.cc
@@ -0,0 +1,15 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actors/can_pickup_action.q.h"
+#include "frc971/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/frc971/frc971.gyp b/frc971/frc971.gyp
index 8eeb249..e8e42b0 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -42,6 +42,7 @@
'<(DEPTH)/frc971/actors/actors.gyp:stack_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:pickup_action_lib',
'<(DEPTH)/frc971/actors/actors.gyp:lift_action_lib',
+ '<(DEPTH)/frc971/actors/actors.gyp:can_pickup_action_lib',
],
},
],
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 2bdf5c0..9fd4441 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -21,6 +21,7 @@
#include "frc971/actors/pickup_actor.h"
#include "frc971/actors/stack_actor.h"
#include "frc971/actors/lift_actor.h"
+#include "frc971/actors/can_pickup_actor.h"
using ::frc971::control_loops::claw_queue;
using ::frc971::control_loops::drivetrain_queue;
@@ -49,6 +50,7 @@
const ButtonLocation kElevatorUp(3, 10);
const ButtonLocation kElevatorDown(3, 3);
const ButtonLocation kArmUp(3, 8);
+const ButtonLocation kCanPickup(3, 8);
const ButtonLocation kArmDown(2, 6);
const ButtonLocation kClawUp(3, 7);
const ButtonLocation kClawDown(3, 6);
@@ -142,6 +144,17 @@
params.claw_out_angle = 0.6;
action_queue_.EnqueueAction(actors::MakeStackAction(params));
}
+
+ if (data.PosEdge(kCanPickup)) {
+ actors::CanPickupParams params;
+ params.pickup_angle = -0.93;
+ params.pickup_height = 0.265;
+ params.lift_height = 0.65;
+ params.end_height = 0.4;
+ params.end_angle = 0.0;
+ action_queue_.EnqueueAction(actors::MakeCanPickupAction(params));
+ }
+
if (data.PosEdge(kPickup)) {
actors::PickupParams params;
params.pickup_angle = 0.7;
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 525ffcd..be8c7d3 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -27,6 +27,7 @@
'../actors/actors.gyp:score_action_test',
'../actors/actors.gyp:pickup_action',
'../actors/actors.gyp:stack_action',
+ '../actors/actors.gyp:can_pickup_action',
'../actors/actors.gyp:lift_action',
'../actors/actors.gyp:intake_action',
'../actors/actors.gyp:intake_action_test',