Add target hints to auto and a far-rocket hint
Change-Id: If23c46eca23cd29e3200084889fda8a35edd7faa
diff --git a/frc971/autonomous/BUILD b/frc971/autonomous/BUILD
index 003ceb6..2d74bf2 100644
--- a/frc971/autonomous/BUILD
+++ b/frc971/autonomous/BUILD
@@ -1,31 +1,33 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
queue_library(
- name = 'auto_queue',
- srcs = [
- 'auto.q',
- ],
- deps = [
- '//aos/actions:action_queue',
- ],
+ name = "auto_queue",
+ srcs = [
+ "auto.q",
+ ],
+ deps = [
+ "//aos/actions:action_queue",
+ ],
)
cc_library(
- name = 'base_autonomous_actor',
- hdrs = [
- 'base_autonomous_actor.h',
- ],
- srcs = [
- 'base_autonomous_actor.cc',
- ],
- deps = [
- ':auto_queue',
- '//aos/actions:action_lib',
- '//aos/util:phased_loop',
- '//aos/logging',
- '//frc971/control_loops/drivetrain:drivetrain_config',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- ],
+ name = "base_autonomous_actor",
+ srcs = [
+ "base_autonomous_actor.cc",
+ ],
+ hdrs = [
+ "base_autonomous_actor.h",
+ ],
+ deps = [
+ ":auto_queue",
+ "//aos/actions:action_lib",
+ "//aos/logging",
+ "//aos/util:phased_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:localizer_queue",
+ "//y2019/control_loops/drivetrain:target_selector_queue",
+ ],
)
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index a71ee7d..3509d63 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -9,9 +9,12 @@
#include "aos/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "y2019/control_loops/drivetrain/target_selector.q.h"
using ::frc971::control_loops::drivetrain_queue;
using ::aos::monotonic_clock;
+using ::y2019::control_loops::drivetrain::target_selector_hint;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
@@ -375,7 +378,7 @@
}
}
-void BaseAutonomousActor::LineFollowAtVelocity(double velocity) {
+void BaseAutonomousActor::LineFollowAtVelocity(double velocity, int hint) {
auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
drivetrain_message->controller_type = 3;
// TODO(james): Currently the 4.0 is copied from the
@@ -383,6 +386,9 @@
// factor it out in some way.
drivetrain_message->throttle = velocity / 4.0;
drivetrain_message.Send();
+ auto target_hint = target_selector_hint.MakeMessage();
+ target_hint->suggested_target = hint;
+ target_hint.Send();
}
BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index e060a2c..581e107 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -70,7 +70,7 @@
// Returns true if the drive has finished.
bool IsDriveDone();
- void LineFollowAtVelocity(double velocity);
+ void LineFollowAtVelocity(double velocity, int hint = 0);
// Waits until the robot is pitched up above the specified angle, or the move
// finishes. Returns true on success, and false if it cancels.
diff --git a/y2019/control_loops/drivetrain/target_selector.cc b/y2019/control_loops/drivetrain/target_selector.cc
index 7b6918a..a338181 100644
--- a/y2019/control_loops/drivetrain/target_selector.cc
+++ b/y2019/control_loops/drivetrain/target_selector.cc
@@ -81,6 +81,11 @@
continue;
}
break;
+ case SelectionHint::kFarRocket:
+ if (view.target->target_type() != Target::TargetType::kFarRocket) {
+ continue;
+ }
+ break;
case SelectionHint::kNone:
default:
break;
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index 0086f5d..7f86f89 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -36,6 +36,7 @@
kNearShip = 1,
kMidShip = 2,
kFarShip = 3,
+ kFarRocket = 4,
};
TargetSelector(::aos::EventLoop *event_loop);
diff --git a/y2019/control_loops/drivetrain/target_selector.q b/y2019/control_loops/drivetrain/target_selector.q
index c917d39..7ad2013 100644
--- a/y2019/control_loops/drivetrain/target_selector.q
+++ b/y2019/control_loops/drivetrain/target_selector.q
@@ -5,6 +5,7 @@
// Which target we should go for:
// 0 implies no selection, we should just default to whatever.
// 1, 2, and 3 imply the near, middle, and far targets.
+ // 4 implies far side of the rocket ship.
// These should match the SelectionHint enum in target_selector.h.
uint8_t suggested_target;
};