Merge "various formatting/comment fixups"
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 0cd1c9b..f4fe4f6 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -213,5 +213,77 @@
'stack_action_lib',
],
},
+ {
+ 'target_name': 'claw_action_queue',
+ 'type': 'static_library',
+ 'sources': ['claw_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': 'claw_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'claw_actor.cc',
+ ],
+ 'dependencies': [
+ 'claw_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(EXTERNALS):eigen',
+ '<(AOS)/common/util/util.gyp:trapezoid_profile',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):eigen',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'claw_action_queue',
+ ],
+ },
+ {
+ 'target_name': 'claw_action',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_actor_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'claw_action_queue',
+ 'claw_action_lib',
+ ],
+ },
+ {
+ 'target_name': 'claw_action_test',
+ 'type': 'executable',
+ 'sources': [
+ 'claw_actor_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/common.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
+ 'claw_action_queue',
+ 'claw_action_lib',
+ ],
+ },
],
}
diff --git a/frc971/actors/claw_action.q b/frc971/actors/claw_action.q
new file mode 100644
index 0000000..08b463f
--- /dev/null
+++ b/frc971/actors/claw_action.q
@@ -0,0 +1,25 @@
+package frc971.actors;
+
+import "aos/common/actions/actions.q";
+
+struct ClawParams {
+ double claw_angle;
+ double claw_max_velocity;
+ // Positive is sucking in, negative is spitting out.
+ double intake_voltage;
+ bool rollers_closed;
+};
+
+queue_group ClawActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ ClawParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group ClawActionQueueGroup claw_action;
diff --git a/frc971/actors/claw_actor.cc b/frc971/actors/claw_actor.cc
new file mode 100644
index 0000000..1cba2c0
--- /dev/null
+++ b/frc971/actors/claw_actor.cc
@@ -0,0 +1,100 @@
+#include <functional>
+#include <numeric>
+
+#include <Eigen/Dense>
+
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/actions/actor.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+#include "frc971/constants.h"
+#include "frc971/actors/claw_actor.h"
+#include "frc971/control_loops/claw/claw.q.h"
+
+namespace frc971 {
+namespace actors {
+namespace {
+
+// Defines finished.
+constexpr double kAngleEpsilon = 0.01;
+
+} // namespace
+
+ClawActor::ClawActor(actors::ClawActionQueueGroup* s)
+ : aos::common::actions::ActorBase<actors::ClawActionQueueGroup>(s) {}
+
+bool ClawActor::Iterate(const ClawParams& params) {
+ const double goal_angle = params.claw_angle;
+ const double goal_velocity = params.claw_max_velocity;
+
+ if (::std::abs(claw_start_angle_ + delta_angle_ - goal_angle) >
+ kAngleEpsilon) {
+ delta_angle_ += goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
+ } else {
+ delta_angle_ = goal_angle - claw_start_angle_;
+ }
+
+ auto message = control_loops::claw_queue.goal.MakeMessage();
+ message->angle = claw_start_angle_ + delta_angle_;
+ message->angular_velocity = goal_velocity;
+ message->intake = params.intake_voltage;
+ message->rollers_closed = params.rollers_closed;
+
+ LOG_STRUCT(DEBUG, "Sending claw goal", *message);
+ message.Send();
+
+ control_loops::claw_queue.status.FetchLatest();
+ if (!control_loops::claw_queue.status.get()) {
+ return false;
+ }
+ const double current_angle = control_loops::claw_queue.status->angle;
+ LOG_STRUCT(DEBUG, "Got claw status", *control_loops::claw_queue.status);
+
+ if (::std::abs(goal_angle - current_angle) <
+ kAngleEpsilon) {
+ return true;
+ }
+
+ return false;
+}
+
+bool ClawActor::RunAction(const ClawParams& params) {
+ LOG(INFO, "Claw goal (angle, velocity): %f, %f\n", params.claw_angle,
+ params.claw_max_velocity);
+
+ control_loops::claw_queue.status.FetchLatest();
+ if (control_loops::claw_queue.status.get()) {
+ if (!control_loops::claw_queue.status->zeroed) {
+ LOG(ERROR, "We are not running actions on an unzeroed claw!\n");
+ return false;
+ }
+ claw_start_angle_ = control_loops::claw_queue.status->angle;
+ } else {
+ LOG(ERROR, "No claw status!\n");
+ return false;
+ }
+
+ delta_angle_ = 0.0;
+ while (!Iterate(params)) {
+ // wait until next Xms tick
+ ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+
+ // check if we should stop before we send
+ if (ShouldCancel()) return true;
+ }
+
+ LOG(INFO, "Claw done moving.\n");
+ return true;
+}
+
+::std::unique_ptr<ClawAction> MakeClawAction(const ClawParams& p) {
+ return ::std::unique_ptr<ClawAction>(
+ new ClawAction(&::frc971::actors::claw_action, p));
+}
+
+} // namespace actors
+} // namespace frc971
diff --git a/frc971/actors/claw_actor.h b/frc971/actors/claw_actor.h
new file mode 100644
index 0000000..9885d3d
--- /dev/null
+++ b/frc971/actors/claw_actor.h
@@ -0,0 +1,40 @@
+#ifndef FRC971_ACTIONS_CLAW_ACTION_H_
+#define FRC971_ACTIONS_CLAW_ACTION_H_
+
+#include <memory>
+
+#include "frc971/actors/claw_action.q.h"
+#include "aos/common/actions/actor.h"
+#include "aos/common/actions/actions.h"
+#include "aos/common/util/trapezoid_profile.h"
+
+namespace frc971 {
+namespace actors {
+namespace testing {
+class ClawActorTest_ValidGoals_Test;
+}
+
+class ClawActor : public aos::common::actions::ActorBase<ClawActionQueueGroup> {
+ public:
+ explicit ClawActor(ClawActionQueueGroup *s);
+ bool RunAction(const ClawParams ¶ms) override;
+
+ private:
+ friend class testing::ClawActorTest_ValidGoals_Test;
+
+ // Returns true if it's reached its ultimate goal, false otherwise.
+ bool Iterate(const ClawParams ¶ms);
+
+ double delta_angle_ = 0.0;
+ double claw_start_angle_ = 0.0;
+};
+
+typedef aos::common::actions::TypedAction<ClawActionQueueGroup> ClawAction;
+
+// Makes a new FridgeProfileActor action.
+::std::unique_ptr<ClawAction> MakeClawAction(const ClawParams &claw_params);
+
+} // namespace actors
+} // namespace frc971
+
+#endif
diff --git a/frc971/actors/claw_actor_main.cc b/frc971/actors/claw_actor_main.cc
new file mode 100644
index 0000000..d0cd0d5
--- /dev/null
+++ b/frc971/actors/claw_actor_main.cc
@@ -0,0 +1,19 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actors/claw_action.q.h"
+#include "frc971/actors/claw_actor.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actors::ClawActor claw(
+ &::frc971::actors::claw_action);
+ claw.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/actors/claw_actor_test.cc b/frc971/actors/claw_actor_test.cc
new file mode 100644
index 0000000..7c0cf4e
--- /dev/null
+++ b/frc971/actors/claw_actor_test.cc
@@ -0,0 +1,98 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/actions/actor.h"
+#include "frc971/actors/claw_action.q.h"
+#include "frc971/actors/claw_actor.h"
+#include "frc971/control_loops/claw/claw.q.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace actors {
+namespace testing {
+
+class ClawActorTest : public ::testing::Test {
+ protected:
+ ClawActorTest() {
+ frc971::actors::claw_action.goal.Clear();
+ frc971::actors::claw_action.status.Clear();
+ control_loops::claw_queue.status.Clear();
+ control_loops::claw_queue.goal.Clear();
+ }
+
+ virtual ~ClawActorTest() {
+ frc971::actors::claw_action.goal.Clear();
+ frc971::actors::claw_action.status.Clear();
+ control_loops::claw_queue.status.Clear();
+ control_loops::claw_queue.goal.Clear();
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+};
+
+// Tests that it runs normally and exits when it should.
+TEST_F(ClawActorTest, BasicTest) {
+ ClawActor claw(&frc971::actors::claw_action);
+
+ // Make some reasonable parameters.
+ ClawParams params;
+ params.claw_angle = 0.5;
+ params.claw_max_velocity = 0.5;
+
+ // Fake the status so that it thinks we're already there.
+ control_loops::claw_queue.status.MakeWithBuilder()
+ .angle(0.5)
+ .zeroed(true)
+ .Send();
+
+ // Now we run it and it should exit immediately.
+ EXPECT_TRUE(claw.RunAction(params));
+
+ // It shouldn't have sent us anywhere.
+ ASSERT_TRUE(control_loops::claw_queue.goal.FetchLatest());
+ EXPECT_EQ(params.claw_angle, control_loops::claw_queue.goal->angle);
+}
+
+// Tests that it's outputting the goals we expect it to.
+TEST_F(ClawActorTest, ValidGoals) {
+ ClawActor claw(&frc971::actors::claw_action);
+
+ // Make some reasonable parameters.
+ ClawParams params;
+ params.claw_angle = 0.5;
+ params.claw_max_velocity = 0.5;
+
+ // Set the starting parameters to what we want them to be.
+ claw.delta_angle_ = 0.0;
+ claw.claw_start_angle_ = 0.0;
+
+ // Do the action iteration by iteration.
+ double delta_goal = 0.0;
+ while (!claw.Iterate(params)) {
+ // Check that it sent a reasonable goal.
+ control_loops::claw_queue.goal.FetchLatest();
+ ASSERT_TRUE(control_loops::claw_queue.goal.get() != nullptr);
+
+ delta_goal +=
+ params.claw_max_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
+ EXPECT_EQ(delta_goal, control_loops::claw_queue.goal->angle);
+
+ // Fake a status.
+ control_loops::claw_queue.status.MakeWithBuilder().angle(delta_goal).Send();
+ }
+
+ delta_goal +=
+ params.claw_max_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
+ EXPECT_NEAR(params.claw_angle, delta_goal, 0.01);
+}
+
+} // namespace testing.
+} // namespace actors.
+} // namespace frc971.
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 293fa21..decd7a2 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -24,6 +24,8 @@
'../actors/actors.gyp:score_action',
'../actors/actors.gyp:stack_action',
'../actors/actors.gyp:fridge_profile_action_test',
+ '../actors/actors.gyp:claw_action',
+ '../actors/actors.gyp:claw_action_test',
],
'copies': [
{