Added pickup action.
Change-Id: Ide6931b32b489577c05bb890f7dfcf5f2b35c71d
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 90b57b2..30bb318 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -188,7 +188,53 @@
'score_action_lib',
],
},
-
+ {
+ 'target_name': 'pickup_action_queue',
+ 'type': 'static_library',
+ 'sources': ['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': 'pickup_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'pickup_actor.cc',
+ ],
+ 'dependencies': [
+ 'fridge_profile_action_lib',
+ 'pickup_action_queue',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/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': 'stack_action_queue',
'type': 'static_library',
diff --git a/frc971/actors/pickup_action.q b/frc971/actors/pickup_action.q
new file mode 100644
index 0000000..237337a
--- /dev/null
+++ b/frc971/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/frc971/actors/pickup_actor.cc b/frc971/actors/pickup_actor.cc
new file mode 100644
index 0000000..ef32c92
--- /dev/null
+++ b/frc971/actors/pickup_actor.cc
@@ -0,0 +1,115 @@
+#include "frc971/actors/pickup_actor.h"
+
+#include <math.h>
+
+#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 "frc971/actors/fridge_profile_actor.h"
+#include "frc971/constants.h"
+#include "frc971/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+
+PickupActor::PickupActor(PickupActionQueueGroup* queues)
+ : aos::common::actions::ActorBase<PickupActionQueueGroup>(queues) {}
+
+bool PickupActor::RunAction(const PickupParams& params) {
+ constexpr double kAngleEpsilon = 0.05;
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.pickup_angle;
+ 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;
+ }
+ }
+
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.pickup_angle;
+ 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;
+ }
+ }
+
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.suck_angle;
+ message->angular_velocity = 0.0;
+ message->intake = params.intake_voltage;
+ message->rollers_closed = true;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+ }
+
+ ::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;
+ }
+
+ {
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = params.pickup_finish_angle;
+ 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 (::std::abs(current_angle - params.pickup_finish_angle) <
+ kAngleEpsilon) {
+ break;
+ }
+ }
+
+ 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/frc971/actors/pickup_actor.h b/frc971/actors/pickup_actor.h
new file mode 100644
index 0000000..66988d1
--- /dev/null
+++ b/frc971/actors/pickup_actor.h
@@ -0,0 +1,27 @@
+#ifndef FRC971_ACTORS_PICKUP_ACTOR_H_
+#define FRC971_ACTORS_PICKUP_ACTOR_H_
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/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 // FRC971_ACTORS_PICKUP_ACTOR_H_
diff --git a/frc971/actors/pickup_actor_main.cc b/frc971/actors/pickup_actor_main.cc
new file mode 100644
index 0000000..d5bc169
--- /dev/null
+++ b/frc971/actors/pickup_actor_main.cc
@@ -0,0 +1,15 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actors/pickup_action.q.h"
+#include "frc971/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/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index baeb5a1..086fe25 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -24,6 +24,7 @@
'../actors/actors.gyp:fridge_profile_action',
'../actors/actors.gyp:score_action',
'../actors/actors.gyp:score_action_test',
+ '../actors/actors.gyp:pickup_action',
'../actors/actors.gyp:stack_action',
'../actors/actors.gyp:lift_action',
'../actors/actors.gyp:intake_action',