Added a vision alignment actor.
Change-Id: I6aa7c0f13c4f1b6775e04b8bd5838a564bcba8c0
diff --git a/y2016/BUILD b/y2016/BUILD
index 71bdace..9bc5cd7 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -38,6 +38,7 @@
'//frc971/control_loops/drivetrain:drivetrain_queue',
'//frc971/queues:gyro',
'//y2016/actors:autonomous_action_lib',
+ '//y2016/actors:vision_align_action_lib',
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_lib',
'//y2016/control_loops/superstructure:superstructure_queue',
@@ -54,7 +55,9 @@
'//y2016/control_loops/superstructure:superstructure',
'//y2016/control_loops/shooter:shooter',
'//y2016/actors:autonomous_action',
+ '//y2016/actors:vision_align_action',
'//y2016/wpilib:wpilib_interface',
+ '//y2016/vision:target_receiver',
],
srcs = [
'//aos:prime_binaries',
@@ -70,7 +73,9 @@
'//y2016/control_loops/superstructure:superstructure.stripped',
'//y2016/control_loops/shooter:shooter.stripped',
'//y2016/actors:autonomous_action.stripped',
+ '//y2016/actors:vision_align_action.stripped',
'//y2016/wpilib:wpilib_interface.stripped',
+ '//y2016/vision:target_receiver.stripped',
],
srcs = [
'//aos:prime_binaries_stripped',
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index 771e6ef..6c83721 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -134,3 +134,50 @@
'//aos/linux_code:init',
],
)
+
+queue_library(
+ name = 'vision_align_action_queue',
+ srcs = [
+ 'vision_align_action.q',
+ ],
+ deps = [
+ '//aos/common/actions:action_queue',
+ ],
+)
+
+cc_library(
+ name = 'vision_align_action_lib',
+ srcs = [
+ 'vision_align_actor.cc',
+ ],
+ hdrs = [
+ 'vision_align_actor.h',
+ ],
+ deps = [
+ ':vision_align_action_queue',
+ '//aos/common:time',
+ '//aos/common:math',
+ '//aos/common/util:phased_loop',
+ '//aos/common/logging',
+ '//aos/common/actions:action_lib',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/util:trapezoid_profile',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//third_party/eigen',
+ '//y2016:constants',
+ '//y2016/control_loops/drivetrain:drivetrain_base',
+ '//y2016/vision:vision_queue',
+ ],
+)
+
+cc_binary(
+ name = 'vision_align_action',
+ srcs = [
+ 'vision_align_actor_main.cc',
+ ],
+ deps = [
+ ':vision_align_action_lib',
+ ':vision_align_action_queue',
+ '//aos/linux_code:init',
+ ],
+)
diff --git a/y2016/actors/vision_align_action.q b/y2016/actors/vision_align_action.q
new file mode 100644
index 0000000..742d936
--- /dev/null
+++ b/y2016/actors/vision_align_action.q
@@ -0,0 +1,22 @@
+package y2016.actors;
+
+import "aos/common/actions/actions.q";
+
+// Parameters to send with start.
+struct VisionAlignActionParams {
+ int32_t run;
+};
+
+queue_group VisionAlignActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ VisionAlignActionParams params;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group VisionAlignActionQueueGroup vision_align_action;
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
new file mode 100644
index 0000000..60e0b7c
--- /dev/null
+++ b/y2016/actors/vision_align_actor.cc
@@ -0,0 +1,91 @@
+#include "y2016/actors/vision_align_actor.h"
+
+#include <functional>
+#include <numeric>
+
+#include <Eigen/Dense>
+
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "aos/common/commonmath.h"
+#include "aos/common/time.h"
+
+#include "y2016/actors/vision_align_actor.h"
+#include "y2016/constants.h"
+#include "y2016/vision/vision.q.h"
+#include "y2016/control_loops/drivetrain/drivetrain_base.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+namespace y2016 {
+namespace actors {
+using ::frc971::control_loops::drivetrain_queue;
+
+VisionAlignActor::VisionAlignActor(actors::VisionAlignActionQueueGroup *s)
+ : aos::common::actions::ActorBase<actors::VisionAlignActionQueueGroup>(s) {}
+
+bool VisionAlignActor::RunAction(
+ const actors::VisionAlignActionParams & /*params*/) {
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(5) / 2);
+ while (true) {
+ const int iterations = phased_loop.SleepUntilNext();
+ if (iterations != 1) {
+ LOG(WARNING, "vision align actor skipped %d iterations\n",
+ iterations - 1);
+ }
+
+ if (ShouldCancel()) {
+ return true;
+ }
+ if (!::y2016::vision::vision_status.FetchLatest()) {
+ continue;
+ }
+
+ if (!::y2016::vision::vision_status->left_image_valid ||
+ !::y2016::vision::vision_status->right_image_valid) {
+ continue;
+ }
+
+ const double side_distance_change =
+ ::y2016::vision::vision_status->horizontal_angle *
+ control_loops::GetDrivetrainConfig().robot_radius;
+ if (!::std::isfinite(side_distance_change)) {
+ continue;
+ }
+
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ const double left_current =
+ drivetrain_queue.status->estimated_left_position;
+ const double right_current =
+ drivetrain_queue.status->estimated_right_position;
+
+ if (!drivetrain_queue.goal.MakeWithBuilder()
+ .steering(0.0)
+ .throttle(0.0)
+ .highgear(false)
+ .quickturn(false)
+ .control_loop_driving(true)
+ .left_goal(left_current + side_distance_change)
+ .right_goal(right_current - side_distance_change)
+ .left_velocity_goal(0)
+ .right_velocity_goal(0)
+ .Send()) {
+ LOG(WARNING, "sending drivetrain goal failed\n");
+ }
+ }
+ }
+
+ LOG(INFO, "Done moving\n");
+ return true;
+}
+
+::std::unique_ptr<VisionAlignAction> MakeVisionAlignAction(
+ const ::y2016::actors::VisionAlignActionParams ¶ms) {
+ return ::std::unique_ptr<VisionAlignAction>(
+ new VisionAlignAction(&::y2016::actors::vision_align_action, params));
+}
+
+} // namespace actors
+} // namespace y2016
diff --git a/y2016/actors/vision_align_actor.h b/y2016/actors/vision_align_actor.h
new file mode 100644
index 0000000..5c9a812
--- /dev/null
+++ b/y2016/actors/vision_align_actor.h
@@ -0,0 +1,33 @@
+#ifndef Y2016_ACTORS_DRIVETRAIN_ACTOR_H_
+#define Y2016_ACTORS_DRIVETRAIN_ACTOR_H_
+
+#include <memory>
+
+#include "aos/common/actions/actor.h"
+#include "aos/common/actions/actions.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "y2016/actors/vision_align_action.q.h"
+
+namespace y2016 {
+namespace actors {
+
+class VisionAlignActor
+ : public ::aos::common::actions::ActorBase<VisionAlignActionQueueGroup> {
+ public:
+ explicit VisionAlignActor(VisionAlignActionQueueGroup *s);
+
+ bool RunAction(const actors::VisionAlignActionParams ¶ms) override;
+};
+
+typedef ::aos::common::actions::TypedAction<VisionAlignActionQueueGroup>
+ VisionAlignAction;
+
+// Makes a new VisionAlignActor action.
+::std::unique_ptr<VisionAlignAction> MakeVisionAlignAction(
+ const ::y2016::actors::VisionAlignActionParams ¶ms);
+
+} // namespace actors
+} // namespace y2016
+
+#endif
diff --git a/y2016/actors/vision_align_actor_main.cc b/y2016/actors/vision_align_actor_main.cc
new file mode 100644
index 0000000..ccd4734
--- /dev/null
+++ b/y2016/actors/vision_align_actor_main.cc
@@ -0,0 +1,18 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "y2016/actors/vision_align_action.q.h"
+#include "y2016/actors/vision_align_actor.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char* /*argv*/ []) {
+ ::aos::Init(-1);
+
+ ::y2016::actors::VisionAlignActor vision_align(
+ &::y2016::actors::vision_align_action);
+ vision_align.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index df8fd7d..87ebaf6 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -21,6 +21,7 @@
#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
#include "y2016/actors/autonomous_actor.h"
+#include "y2016/actors/vision_align_actor.h"
using ::frc971::control_loops::drivetrain_queue;
using ::y2016::control_loops::shooter::shooter_queue;
@@ -62,6 +63,8 @@
const ButtonLocation kTest7(3, 5);
const ButtonLocation kIntakeOut(3, 9);
+const ButtonLocation kVisionAlign(1, 6);
+
class Reader : public ::aos::input::JoystickInput {
public:
Reader()
@@ -107,6 +110,20 @@
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
+ if (data.PosEdge(kVisionAlign)) {
+ actors::VisionAlignActionParams params;
+ action_queue_.EnqueueAction(actors::MakeVisionAlignAction(params));
+ }
+
+ if (data.NegEdge(kVisionAlign)) {
+ action_queue_.CancelAllActions();
+ }
+
+ // Don't do any normal drivetrain stuff if vision is in charge.
+ if (was_running_) {
+ return;
+ }
+
if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
drivetrain_queue.status.FetchLatest();
if (drivetrain_queue.status.get()) {