Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/y2014/BUILD b/y2014/BUILD
index b11f170..1e1384b 100644
--- a/y2014/BUILD
+++ b/y2014/BUILD
@@ -1,4 +1,5 @@
load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
cc_library(
name = "constants",
@@ -33,13 +34,14 @@
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/queues:gyro",
"//y2014/actors:shoot_action_lib",
- "//y2014/control_loops/claw:claw_queue",
+ "//y2014/control_loops/claw:claw_goal_fbs",
+ "//y2014/control_loops/claw:claw_status_fbs",
"//y2014/control_loops/drivetrain:drivetrain_base",
- "//y2014/control_loops/shooter:shooter_queue",
+ "//y2014/control_loops/shooter:shooter_goal_fbs",
],
)
@@ -61,12 +63,32 @@
"hot_goal_reader.cc",
],
deps = [
+ "//aos:byteorder",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/time",
- "//y2014/queues:hot_goal",
+ "//y2014/queues:hot_goal_fbs",
+ ],
+)
+
+aos_config(
+ name = "config",
+ src = "y2014.json",
+ flatbuffers = [
+ "//y2014/control_loops/shooter:shooter_goal_fbs",
+ "//y2014/control_loops/shooter:shooter_output_fbs",
+ "//y2014/control_loops/shooter:shooter_position_fbs",
+ "//y2014/control_loops/shooter:shooter_status_fbs",
+ "//y2014/control_loops/claw:claw_goal_fbs",
+ "//y2014/control_loops/claw:claw_output_fbs",
+ "//y2014/control_loops/claw:claw_position_fbs",
+ "//y2014/control_loops/claw:claw_status_fbs",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/control_loops/drivetrain:config",
],
)
@@ -82,15 +104,15 @@
"//aos:make_unique",
"//aos/controls:control_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
"//frc971/wpilib:dma_edge_counting",
@@ -99,16 +121,18 @@
"//frc971/wpilib:gyro_sender",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
- "//y2014/control_loops/claw:claw_queue",
- "//y2014/control_loops/shooter:shooter_queue",
- "//y2014/queues:auto_mode",
+ "//y2014/control_loops/claw:claw_output_fbs",
+ "//y2014/control_loops/claw:claw_position_fbs",
+ "//y2014/control_loops/shooter:shooter_output_fbs",
+ "//y2014/control_loops/shooter:shooter_position_fbs",
+ "//y2014/queues:auto_mode_fbs",
],
)
diff --git a/y2014/actors/BUILD b/y2014/actors/BUILD
index 8612d0b..82255ab 100644
--- a/y2014/actors/BUILD
+++ b/y2014/actors/BUILD
@@ -1,5 +1,3 @@
-load("//aos/build:queues.bzl", "queue_library")
-
filegroup(
name = "binaries",
srcs = [
@@ -18,16 +16,6 @@
visibility = ["//visibility:public"],
)
-queue_library(
- name = "shoot_action_queue",
- srcs = [
- "shoot_action.q",
- ],
- deps = [
- "//aos/actions:action_queue",
- ],
-)
-
cc_library(
name = "shoot_action_lib",
srcs = [
@@ -38,14 +26,13 @@
],
visibility = ["//visibility:public"],
deps = [
- ":shoot_action_queue",
"//aos/actions:action_lib",
"//aos/logging",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2014:constants",
- "//y2014/control_loops/claw:claw_queue",
- "//y2014/control_loops/shooter:shooter_queue",
- "//y2014/queues:profile_params",
+ "//y2014/control_loops/claw:claw_goal_fbs",
+ "//y2014/control_loops/claw:claw_status_fbs",
+ "//y2014/control_loops/shooter:shooter_goal_fbs",
+ "//y2014/control_loops/shooter:shooter_status_fbs",
],
)
@@ -56,9 +43,8 @@
],
deps = [
":shoot_action_lib",
- ":shoot_action_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
@@ -72,19 +58,18 @@
],
deps = [
"//aos/actions:action_lib",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2014/actors:shoot_action_lib",
- "//y2014/control_loops/claw:claw_queue",
+ "//y2014/control_loops/claw:claw_goal_fbs",
+ "//y2014/control_loops/claw:claw_status_fbs",
"//y2014/control_loops/drivetrain:drivetrain_base",
- "//y2014/control_loops/shooter:shooter_queue",
- "//y2014/queues:auto_mode",
- "//y2014/queues:hot_goal",
- "//y2014/queues:profile_params",
+ "//y2014/control_loops/shooter:shooter_goal_fbs",
+ "//y2014/queues:auto_mode_fbs",
+ "//y2014/queues:hot_goal_fbs",
],
)
@@ -96,7 +81,6 @@
deps = [
":autonomous_action_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/autonomous:auto_queue",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014/actors/autonomous_actor.cc b/y2014/actors/autonomous_actor.cc
index 4e2c9aa..7d1eb3f 100644
--- a/y2014/actors/autonomous_actor.cc
+++ b/y2014/actors/autonomous_actor.cc
@@ -7,18 +7,16 @@
#include "aos/actions/actions.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/time/time.h"
#include "aos/util/phased_loop.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/actors/shoot_actor.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
#include "y2014/control_loops/drivetrain/drivetrain_base.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/queues/auto_mode.q.h"
-#include "y2014/queues/hot_goal.q.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/queues/auto_mode_generated.h"
+#include "y2014/queues/hot_goal_generated.h"
namespace y2014 {
namespace actors {
@@ -26,49 +24,50 @@
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
using ::aos::monotonic_clock;
-using ::frc971::ProfileParameters;
+using ::frc971::ProfileParametersT;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
: frc971::autonomous::BaseAutonomousActor(
event_loop, control_loops::GetDrivetrainConfig()),
- auto_mode_fetcher_(event_loop->MakeFetcher<::y2014::sensors::AutoMode>(
- ".y2014.sensors.auto_mode")),
- hot_goal_fetcher_(
- event_loop->MakeFetcher<::y2014::HotGoal>(".y2014.hot_goal")),
+ auto_mode_fetcher_(
+ event_loop->MakeFetcher<::y2014::sensors::AutoMode>("/aos")),
+ hot_goal_fetcher_(event_loop->MakeFetcher<::y2014::HotGoal>("/")),
claw_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::claw::Goal>("/claw")),
claw_goal_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Goal>("/claw")),
claw_status_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Status>(
- ".y2014.control_loops.claw_queue.status")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Status>(
+ "/claw")),
shooter_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::shooter::Goal>(
+ "/shooter")),
shoot_action_factory_(actors::ShootActor::MakeFactory(event_loop)) {}
void AutonomousActor::PositionClawVertically(double intake_power,
double centering_power) {
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = 0.0;
- goal_message->separation_angle = 0.0;
- goal_message->intake = intake_power;
- goal_message->centering = centering_power;
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(0.0);
+ goal_builder.add_separation_angle(0.0);
+ goal_builder.add_intake(intake_power);
+ goal_builder.add_centering(centering_power);
- if (!goal_message.Send()) {
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
void AutonomousActor::PositionClawBackIntake() {
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = -2.273474;
- goal_message->separation_angle = 0.0;
- goal_message->intake = 12.0;
- goal_message->centering = 12.0;
- if (!goal_message.Send()) {
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(-2.273474);
+ goal_builder.add_separation_angle(0.0);
+ goal_builder.add_intake(12.0);
+ goal_builder.add_centering(12.0);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
@@ -76,48 +75,63 @@
void AutonomousActor::PositionClawUpClosed() {
// Move the claw to where we're going to shoot from but keep it closed until
// it gets there.
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = 0.86;
- goal_message->separation_angle = 0.0;
- goal_message->intake = 4.0;
- goal_message->centering = 1.0;
- if (!goal_message.Send()) {
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(0.86);
+ goal_builder.add_separation_angle(0.0);
+ goal_builder.add_intake(4.0);
+ goal_builder.add_centering(1.0);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
void AutonomousActor::PositionClawForShot() {
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = 0.86;
- goal_message->separation_angle = 0.10;
- goal_message->intake = 4.0;
- goal_message->centering = 1.0;
- if (!goal_message.Send()) {
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(0.86);
+ goal_builder.add_separation_angle(0.10);
+ goal_builder.add_intake(4.0);
+ goal_builder.add_centering(1.0);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
void AutonomousActor::SetShotPower(double power) {
AOS_LOG(INFO, "Setting shot power to %f\n", power);
- auto goal_message = shooter_goal_sender_.MakeMessage();
- goal_message->shot_power = power;
- goal_message->shot_requested = false;
- goal_message->unload_requested = false;
- goal_message->load_requested = false;
- if (!goal_message.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ control_loops::shooter::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::shooter::Goal>();
+ goal_builder.add_shot_power(power);
+ goal_builder.add_shot_requested(false);
+ goal_builder.add_unload_requested(false);
+ goal_builder.add_load_requested(false);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending shooter goal failed\n");
}
}
-const ProfileParameters kFastDrive = {3.0, 2.5};
-const ProfileParameters kSlowDrive = {2.5, 2.5};
-const ProfileParameters kFastWithBallDrive = {3.0, 2.0};
-const ProfileParameters kSlowWithBallDrive = {2.5, 2.0};
-const ProfileParameters kFastTurn = {3.0, 10.0};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
+
+const ProfileParametersT kFastDrive = MakeProfileParameters(3.0, 2.5);
+const ProfileParametersT kSlowDrive = MakeProfileParameters(2.5, 2.5);
+const ProfileParametersT kFastWithBallDrive = MakeProfileParameters(3.0, 2.0);
+const ProfileParametersT kSlowWithBallDrive = MakeProfileParameters(2.5, 2.0);
+const ProfileParametersT kFastTurn = MakeProfileParameters(3.0, 10.0);
void AutonomousActor::Shoot() {
// Shoot.
- auto shoot_action = shoot_action_factory_.Make(0.0);
+ aos::common::actions::DoubleParamT param;
+ auto shoot_action = shoot_action_factory_.Make(param);
shoot_action->Start();
WaitUntilDoneOrCanceled(::std::move(shoot_action));
}
@@ -138,12 +152,12 @@
claw_goal_fetcher_.get() == nullptr) {
continue;
}
- bool ans = claw_status_fetcher_->zeroed &&
- (::std::abs(claw_status_fetcher_->bottom_velocity) < 1.0) &&
- (::std::abs(claw_status_fetcher_->bottom -
- claw_goal_fetcher_->bottom_angle) < 0.10) &&
- (::std::abs(claw_status_fetcher_->separation -
- claw_goal_fetcher_->separation_angle) < 0.4);
+ bool ans = claw_status_fetcher_->zeroed() &&
+ (::std::abs(claw_status_fetcher_->bottom_velocity()) < 1.0) &&
+ (::std::abs(claw_status_fetcher_->bottom() -
+ claw_goal_fetcher_->bottom_angle()) < 0.10) &&
+ (::std::abs(claw_status_fetcher_->separation() -
+ claw_goal_fetcher_->separation_angle()) < 0.4);
if (ans) {
return true;
}
@@ -160,8 +174,7 @@
void ResetCounts() {
hot_goal_fetcher_->Fetch();
if (hot_goal_fetcher_->get()) {
- start_counts_ = *hot_goal_fetcher_->get();
- AOS_LOG_STRUCT(INFO, "counts reset to", start_counts_);
+ hot_goal_fetcher_->get()->UnPackTo(&start_counts_);
start_counts_valid_ = true;
} else {
AOS_LOG(WARNING, "no hot goal message. ignoring\n");
@@ -169,30 +182,26 @@
}
}
- void Update() {
- hot_goal_fetcher_->Fetch();
- if (hot_goal_fetcher_->get())
- AOS_LOG_STRUCT(INFO, "new counts", *hot_goal_fetcher_->get());
- }
+ void Update() { hot_goal_fetcher_->Fetch(); }
bool left_triggered() const {
if (!start_counts_valid_ || !hot_goal_fetcher_->get()) return false;
- return (hot_goal_fetcher_->get()->left_count - start_counts_.left_count) >
+ return (hot_goal_fetcher_->get()->left_count() - start_counts_.left_count) >
kThreshold;
}
bool right_triggered() const {
if (!start_counts_valid_ || !hot_goal_fetcher_->get()) return false;
- return (hot_goal_fetcher_->get()->right_count - start_counts_.right_count) >
- kThreshold;
+ return (hot_goal_fetcher_->get()->right_count() -
+ start_counts_.right_count) > kThreshold;
}
bool is_left() const {
if (!start_counts_valid_ || !hot_goal_fetcher_->get()) return false;
const uint64_t left_difference =
- hot_goal_fetcher_->get()->left_count - start_counts_.left_count;
+ hot_goal_fetcher_->get()->left_count() - start_counts_.left_count;
const uint64_t right_difference =
- hot_goal_fetcher_->get()->right_count - start_counts_.right_count;
+ hot_goal_fetcher_->get()->right_count() - start_counts_.right_count;
if (left_difference > kThreshold) {
if (right_difference > kThreshold) {
// We've seen a lot of both, so pick the one we've seen the most of.
@@ -210,9 +219,9 @@
bool is_right() const {
if (!start_counts_valid_ || !hot_goal_fetcher_->get()) return false;
const uint64_t left_difference =
- hot_goal_fetcher_->get()->left_count - start_counts_.left_count;
+ hot_goal_fetcher_->get()->left_count() - start_counts_.left_count;
const uint64_t right_difference =
- hot_goal_fetcher_->get()->right_count - start_counts_.right_count;
+ hot_goal_fetcher_->get()->right_count() - start_counts_.right_count;
if (right_difference > kThreshold) {
if (left_difference > kThreshold) {
// We've seen a lot of both, so pick the one we've seen the most of.
@@ -230,14 +239,14 @@
private:
static const uint64_t kThreshold = 5;
- ::y2014::HotGoal start_counts_;
+ ::y2014::HotGoalT start_counts_;
bool start_counts_valid_;
::aos::Fetcher<::y2014::HotGoal> *hot_goal_fetcher_;
};
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams & /*params*/) {
+ const ::frc971::autonomous::AutonomousActionParams * /*params*/) {
enum class AutoVersion : uint8_t {
kStraight,
kDoubleHot,
@@ -261,9 +270,9 @@
static const double kSelectorMin = 0.2, kSelectorMax = 4.4;
const double kSelectorStep = (kSelectorMax - kSelectorMin) / 3.0;
- if (auto_mode_fetcher_->voltage < kSelectorStep + kSelectorMin) {
+ if (auto_mode_fetcher_->voltage() < kSelectorStep + kSelectorMin) {
auto_version = AutoVersion::kSingleHot;
- } else if (auto_mode_fetcher_->voltage < 2 * kSelectorStep + kSelectorMin) {
+ } else if (auto_mode_fetcher_->voltage() < 2 * kSelectorStep + kSelectorMin) {
auto_version = AutoVersion::kStraight;
} else {
auto_version = AutoVersion::kDoubleHot;
@@ -272,9 +281,9 @@
AOS_LOG(INFO, "running auto %" PRIu8 "\n",
static_cast<uint8_t>(auto_version));
- const ProfileParameters &drive_params =
+ const ProfileParametersT drive_params =
(auto_version == AutoVersion::kStraight) ? kFastDrive : kSlowDrive;
- const ProfileParameters &drive_with_ball_params =
+ const ProfileParametersT drive_with_ball_params =
(auto_version == AutoVersion::kStraight) ? kFastWithBallDrive
: kSlowWithBallDrive;
diff --git a/y2014/actors/autonomous_actor.h b/y2014/actors/autonomous_actor.h
index e832980..f1b06bf 100644
--- a/y2014/actors/autonomous_actor.h
+++ b/y2014/actors/autonomous_actor.h
@@ -7,12 +7,11 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "y2014/actors/shoot_actor.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/queues/auto_mode.q.h"
-#include "y2014/queues/hot_goal.q.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/queues/auto_mode_generated.h"
+#include "y2014/queues/hot_goal_generated.h"
namespace y2014 {
namespace actors {
@@ -22,7 +21,7 @@
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset() {
@@ -41,12 +40,10 @@
::aos::Fetcher<::y2014::sensors::AutoMode> auto_mode_fetcher_;
::aos::Fetcher<::y2014::HotGoal> hot_goal_fetcher_;
- ::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Goal> claw_goal_fetcher_;
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Status>
- claw_status_fetcher_;
- ::aos::Sender<::y2014::control_loops::ShooterQueue::Goal>
- shooter_goal_sender_;
+ ::aos::Sender<::y2014::control_loops::claw::Goal> claw_goal_sender_;
+ ::aos::Fetcher<::y2014::control_loops::claw::Goal> claw_goal_fetcher_;
+ ::aos::Fetcher<::y2014::control_loops::claw::Status> claw_status_fetcher_;
+ ::aos::Sender<::y2014::control_loops::shooter::Goal> shooter_goal_sender_;
actors::ShootActor::Factory shoot_action_factory_;
};
diff --git a/y2014/actors/autonomous_actor_main.cc b/y2014/actors/autonomous_actor_main.cc
index 817f0e4..44e10d0 100644
--- a/y2014/actors/autonomous_actor_main.cc
+++ b/y2014/actors/autonomous_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "frc971/autonomous/auto.q.h"
#include "y2014/actors/autonomous_actor.h"
int main(int /*argc*/, char * /*argv*/ []) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2014::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2014/actors/shoot_action.q b/y2014/actors/shoot_action.q
deleted file mode 100644
index 832153c..0000000
--- a/y2014/actors/shoot_action.q
+++ /dev/null
@@ -1,10 +0,0 @@
-package y2014.actors;
-
-import "aos/actions/actions.q";
-
-queue_group ShootActionQueueGroup {
- implements frc971.actions.ActionQueueGroup;
-
- queue aos.common.actions.Goal goal;
- queue aos.common.actions.Status status;
-};
diff --git a/y2014/actors/shoot_actor.cc b/y2014/actors/shoot_actor.cc
index 7fb4c05..58df186 100644
--- a/y2014/actors/shoot_actor.cc
+++ b/y2014/actors/shoot_actor.cc
@@ -4,10 +4,11 @@
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
namespace y2014 {
namespace actors {
@@ -17,26 +18,24 @@
constexpr double ShootActor::kClawShootingSeparationGoal;
ShootActor::ShootActor(::aos::EventLoop *event_loop)
- : ::aos::common::actions::ActorBase<actors::ShootActionQueueGroup>(
- event_loop, ".y2014.actors.shoot_action"),
+ : ::aos::common::actions::ActorBase<aos::common::actions::Goal>(
+ event_loop, "/shoot_action"),
claw_goal_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Goal>("/claw")),
claw_status_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Status>(
- ".y2014.control_loops.claw_queue.status")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Status>(
+ "/claw")),
claw_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::claw::Goal>("/claw")),
shooter_status_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ShooterQueue::Status>(
- ".y2014.control_loops.shooter_queue.status")),
+ event_loop->MakeFetcher<::y2014::control_loops::shooter::Status>(
+ "/shooter")),
shooter_goal_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
+ event_loop->MakeFetcher<::y2014::control_loops::shooter::Goal>(
+ "/shooter")),
shooter_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")) {}
+ event_loop->MakeSender<::y2014::control_loops::shooter::Goal>(
+ "/shooter")) {}
double ShootActor::SpeedToAngleOffset(double speed) {
const constants::Values &values = constants::GetValues();
@@ -52,14 +51,17 @@
// turn it off.
return true;
} else {
- auto goal_message = claw_goal_sender_.MakeMessage();
+ auto builder = claw_goal_sender_.MakeBuilder();
- goal_message->bottom_angle = claw_goal_fetcher_->bottom_angle;
- goal_message->separation_angle = claw_goal_fetcher_->separation_angle;
- goal_message->intake = 0.0;
- goal_message->centering = 0.0;
+ control_loops::claw::Goal::Builder claw_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
- if (!goal_message.Send()) {
+ claw_builder.add_bottom_angle(claw_goal_fetcher_->bottom_angle());
+ claw_builder.add_separation_angle(claw_goal_fetcher_->separation_angle());
+ claw_builder.add_intake(0.0);
+ claw_builder.add_centering(0.0);
+
+ if (!builder.Send(claw_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
return false;
}
@@ -67,7 +69,7 @@
return true;
}
-bool ShootActor::RunAction(const double&) {
+bool ShootActor::RunAction(const aos::common::actions::DoubleParam *) {
InnerRunAction();
// Now do our 'finally' block and make sure that we aren't requesting shots
@@ -76,12 +78,15 @@
if (shooter_goal_fetcher_.get() == nullptr) {
return true;
}
- auto goal_message = shooter_goal_sender_.MakeMessage();
- goal_message->shot_power = shooter_goal_fetcher_->shot_power;
- goal_message->shot_requested = false;
- goal_message->unload_requested = false;
- goal_message->load_requested = false;
- if (!goal_message.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ control_loops::shooter::Goal::Builder shooter_builder =
+ builder.MakeBuilder<control_loops::shooter::Goal>();
+
+ shooter_builder.add_shot_power(shooter_goal_fetcher_->shot_power());
+ shooter_builder.add_shot_requested(false);
+ shooter_builder.add_unload_requested(false);
+ shooter_builder.add_load_requested(false);
+ if (!builder.Send(shooter_builder.Finish())) {
AOS_LOG(WARNING, "sending shooter goal failed\n");
return false;
}
@@ -104,16 +109,18 @@
shooter_status_fetcher_.Fetch();
// Get the number of shots fired up to this point. This should not be updated
// again for another few cycles.
- previous_shots_ = shooter_status_fetcher_->shots;
+ previous_shots_ = shooter_status_fetcher_->shots();
// Shoot!
shooter_goal_fetcher_.Fetch();
{
- auto goal_message = shooter_goal_sender_.MakeMessage();
- goal_message->shot_power = shooter_goal_fetcher_->shot_power;
- goal_message->shot_requested = true;
- goal_message->unload_requested = false;
- goal_message->load_requested = false;
- if (!goal_message.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ control_loops::shooter::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::shooter::Goal>();
+ goal_builder.add_shot_power(shooter_goal_fetcher_->shot_power());
+ goal_builder.add_shot_requested(true);
+ goal_builder.add_unload_requested(false);
+ goal_builder.add_load_requested(false);
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending shooter goal failed\n");
return;
}
@@ -128,20 +135,20 @@
bool ShootActor::ClawIsReady() {
claw_goal_fetcher_.Fetch();
- bool ans = claw_status_fetcher_->zeroed &&
- (::std::abs(claw_status_fetcher_->bottom_velocity) < 0.5) &&
- (::std::abs(claw_status_fetcher_->bottom -
- claw_goal_fetcher_->bottom_angle) < 0.10) &&
- (::std::abs(claw_status_fetcher_->separation -
- claw_goal_fetcher_->separation_angle) < 0.4);
+ bool ans = claw_status_fetcher_->zeroed() &&
+ (::std::abs(claw_status_fetcher_->bottom_velocity()) < 0.5) &&
+ (::std::abs(claw_status_fetcher_->bottom() -
+ claw_goal_fetcher_->bottom_angle()) < 0.10) &&
+ (::std::abs(claw_status_fetcher_->separation() -
+ claw_goal_fetcher_->separation_angle()) < 0.4);
AOS_LOG(DEBUG,
"Claw is %sready zeroed %d bottom_velocity %f bottom %f sep %f\n",
- ans ? "" : "not ", claw_status_fetcher_->zeroed,
- ::std::abs(claw_status_fetcher_->bottom_velocity),
- ::std::abs(claw_status_fetcher_->bottom -
- claw_goal_fetcher_->bottom_angle),
- ::std::abs(claw_status_fetcher_->separation -
- claw_goal_fetcher_->separation_angle));
+ ans ? "" : "not ", claw_status_fetcher_->zeroed(),
+ ::std::abs(claw_status_fetcher_->bottom_velocity()),
+ ::std::abs(claw_status_fetcher_->bottom() -
+ claw_goal_fetcher_->bottom_angle()),
+ ::std::abs(claw_status_fetcher_->separation() -
+ claw_goal_fetcher_->separation_angle()));
return ans;
}
@@ -149,14 +156,14 @@
shooter_goal_fetcher_.Fetch();
AOS_LOG(DEBUG, "Power error is %f - %f -> %f, ready %d\n",
- shooter_status_fetcher_->hard_stop_power,
- shooter_goal_fetcher_->shot_power,
- ::std::abs(shooter_status_fetcher_->hard_stop_power -
- shooter_goal_fetcher_->shot_power),
- shooter_status_fetcher_->ready);
- return (::std::abs(shooter_status_fetcher_->hard_stop_power -
- shooter_goal_fetcher_->shot_power) < 1.0) &&
- shooter_status_fetcher_->ready;
+ shooter_status_fetcher_->hard_stop_power(),
+ shooter_goal_fetcher_->shot_power(),
+ ::std::abs(shooter_status_fetcher_->hard_stop_power() -
+ shooter_goal_fetcher_->shot_power()),
+ shooter_status_fetcher_->ready());
+ return (::std::abs(shooter_status_fetcher_->hard_stop_power() -
+ shooter_goal_fetcher_->shot_power()) < 1.0) &&
+ shooter_status_fetcher_->ready();
}
bool ShootActor::DoneSetupShot() {
@@ -174,7 +181,7 @@
bool ShootActor::DonePreShotOpen() {
claw_status_fetcher_.Fetch();
- if (claw_status_fetcher_->separation > kClawShootingSeparation) {
+ if (claw_status_fetcher_->separation() > kClawShootingSeparation) {
AOS_LOG(INFO, "Opened up enough to shoot.\n");
return true;
}
@@ -184,7 +191,7 @@
bool ShootActor::DoneShot() {
shooter_status_fetcher_.Fetch();
if (shooter_status_fetcher_.get() &&
- shooter_status_fetcher_->shots > previous_shots_) {
+ shooter_status_fetcher_->shots() > previous_shots_) {
AOS_LOG(INFO, "Shot succeeded!\n");
return true;
}
diff --git a/y2014/actors/shoot_actor.h b/y2014/actors/shoot_actor.h
index a4412ff..88a2358 100644
--- a/y2014/actors/shoot_actor.h
+++ b/y2014/actors/shoot_actor.h
@@ -5,29 +5,30 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "y2014/actors/shoot_action.q.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
namespace y2014 {
namespace actors {
class ShootActor
- : public ::aos::common::actions::ActorBase<actors::ShootActionQueueGroup> {
+ : public ::aos::common::actions::ActorBase<aos::common::actions::Goal> {
public:
typedef ::aos::common::actions::TypedActionFactory<
- actors::ShootActionQueueGroup>
+ aos::common::actions::Goal>
Factory;
explicit ShootActor(::aos::EventLoop *event_loop);
static Factory MakeFactory(::aos::EventLoop *event_loop) {
- return Factory(event_loop, ".y2014.actors.shoot_action");
+ return Factory(event_loop, "/shoot_action");
}
// Actually execute the action of moving the claw and shooter into position
// and actually firing them.
- bool RunAction(const double ¶ms) override;
+ bool RunAction(const aos::common::actions::DoubleParam *params) override;
void InnerRunAction();
// calc an offset to our requested shot based on robot speed
@@ -50,16 +51,13 @@
bool ClawIsReady();
private:
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Goal> claw_goal_fetcher_;
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Status>
- claw_status_fetcher_;
- ::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
- ::aos::Fetcher<::y2014::control_loops::ShooterQueue::Status>
+ ::aos::Fetcher<::y2014::control_loops::claw::Goal> claw_goal_fetcher_;
+ ::aos::Fetcher<::y2014::control_loops::claw::Status> claw_status_fetcher_;
+ ::aos::Sender<::y2014::control_loops::claw::Goal> claw_goal_sender_;
+ ::aos::Fetcher<::y2014::control_loops::shooter::Status>
shooter_status_fetcher_;
- ::aos::Fetcher<::y2014::control_loops::ShooterQueue::Goal>
- shooter_goal_fetcher_;
- ::aos::Sender<::y2014::control_loops::ShooterQueue::Goal>
- shooter_goal_sender_;
+ ::aos::Fetcher<::y2014::control_loops::shooter::Goal> shooter_goal_fetcher_;
+ ::aos::Sender<::y2014::control_loops::shooter::Goal> shooter_goal_sender_;
// to track when shot is complete
int previous_shots_;
diff --git a/y2014/actors/shoot_actor_main.cc b/y2014/actors/shoot_actor_main.cc
index 8575783..ef06915 100644
--- a/y2014/actors/shoot_actor_main.cc
+++ b/y2014/actors/shoot_actor_main.cc
@@ -1,14 +1,16 @@
#include <stdio.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "y2014/actors/shoot_action.q.h"
#include "y2014/actors/shoot_actor.h"
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init(-1);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2014::actors::ShootActor shoot(&event_loop);
event_loop.Run();
diff --git a/y2014/control_loops/claw/BUILD b/y2014/control_loops/claw/BUILD
index 6c162e6..670dd76 100644
--- a/y2014/control_loops/claw/BUILD
+++ b/y2014/control_loops/claw/BUILD
@@ -1,16 +1,40 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "claw_queue",
+flatbuffer_cc_library(
+ name = "claw_goal_fbs",
srcs = [
- "claw.q",
+ "claw_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "claw_position_fbs",
+ srcs = [
+ "claw_position.fbs",
],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "claw_output_fbs",
+ srcs = [
+ "claw_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "claw_status_fbs",
+ srcs = [
+ "claw_status.fbs",
+ ],
+ gen_reflections = 1,
)
genrule(
@@ -40,12 +64,13 @@
"-lm",
],
deps = [
- ":claw_queue",
+ ":claw_goal_fbs",
+ ":claw_output_fbs",
+ ":claw_position_fbs",
+ ":claw_status_fbs",
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
- "//aos/logging:matrix_logging",
- "//aos/logging:queue_logging",
"//frc971/control_loops:coerce_goal",
"//frc971/control_loops:hall_effect_tracker",
"//frc971/control_loops:state_feedback_loop",
@@ -58,9 +83,13 @@
srcs = [
"claw_lib_test.cc",
],
+ data = ["//y2014:config.json"],
deps = [
+ ":claw_goal_fbs",
":claw_lib",
- ":claw_queue",
+ ":claw_output_fbs",
+ ":claw_position_fbs",
+ ":claw_status_fbs",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
@@ -76,6 +105,6 @@
deps = [
":claw_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 14dbf11..56ca6b0 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -2,10 +2,7 @@
#include <algorithm>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/logging/matrix_logging.h"
#include "aos/commonmath.h"
#include "y2014/constants.h"
@@ -46,11 +43,11 @@
namespace y2014 {
namespace control_loops {
+namespace claw {
using ::frc971::HallEffectTracker;
using ::y2014::control_loops::claw::kDt;
using ::frc971::control_loops::DoCoerceGoal;
-using ::y2014::control_loops::ClawPositionToLog;
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
@@ -94,7 +91,7 @@
double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
if (::std::abs(u_bottom) > max_voltage || ::std::abs(u_top) > max_voltage) {
- AOS_LOG_MATRIX(DEBUG, "U at start", U());
+ VLOG(1) << "U at start " << U();
// H * U <= k
// U = UPos + UVel
// H * (UPos + UVel) <= k
@@ -116,7 +113,7 @@
position_error << error(0, 0), error(1, 0);
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(2, 0), error(3, 0);
- AOS_LOG_MATRIX(DEBUG, "error", error);
+ VLOG(1) << "error " << error;
const auto &poly = is_zeroing_ ? U_Poly_zeroing_ : U_Poly_;
const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
@@ -183,9 +180,9 @@
}
}
- AOS_LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
+ VLOG(1) << "adjusted_pos_error " << adjusted_pos_error;
mutable_U() = velocity_K * velocity_error + position_K * adjusted_pos_error;
- AOS_LOG_MATRIX(DEBUG, "U is now", U());
+ VLOG(1) << "U is now" << U();
{
const auto values = constants::GetValues().claw;
@@ -221,69 +218,68 @@
encoder_(0.0),
last_encoder_(0.0) {}
-void ZeroedStateFeedbackLoop::SetPositionValues(
- const ::y2014::control_loops::HalfClawPosition &claw) {
- front_.Update(claw.front);
- calibration_.Update(claw.calibration);
- back_.Update(claw.back);
+void ZeroedStateFeedbackLoop::SetPositionValues(const HalfClawPosition *claw) {
+ front_.Update(claw->front());
+ calibration_.Update(claw->calibration());
+ back_.Update(claw->back());
bool any_sensor_triggered = any_triggered();
if (any_sensor_triggered && any_triggered_last_) {
// We are still on the hall effect and nothing has changed.
min_hall_effect_on_angle_ =
- ::std::min(min_hall_effect_on_angle_, claw.position);
+ ::std::min(min_hall_effect_on_angle_, claw->position());
max_hall_effect_on_angle_ =
- ::std::max(max_hall_effect_on_angle_, claw.position);
+ ::std::max(max_hall_effect_on_angle_, claw->position());
} else if (!any_sensor_triggered && !any_triggered_last_) {
// We are still off the hall effect and nothing has changed.
min_hall_effect_off_angle_ =
- ::std::min(min_hall_effect_off_angle_, claw.position);
+ ::std::min(min_hall_effect_off_angle_, claw->position());
max_hall_effect_off_angle_ =
- ::std::max(max_hall_effect_off_angle_, claw.position);
+ ::std::max(max_hall_effect_off_angle_, claw->position());
}
if (front_.is_posedge()) {
// Saw a posedge on the hall effect. Reset the limits.
min_hall_effect_on_angle_ =
- ::std::min(claw.front.posedge_value, claw.position);
+ ::std::min(claw->front()->posedge_value(), claw->position());
max_hall_effect_on_angle_ =
- ::std::max(claw.front.posedge_value, claw.position);
+ ::std::max(claw->front()->posedge_value(), claw->position());
}
if (calibration_.is_posedge()) {
// Saw a posedge on the hall effect. Reset the limits.
min_hall_effect_on_angle_ =
- ::std::min(claw.calibration.posedge_value, claw.position);
+ ::std::min(claw->calibration()->posedge_value(), claw->position());
max_hall_effect_on_angle_ =
- ::std::max(claw.calibration.posedge_value, claw.position);
+ ::std::max(claw->calibration()->posedge_value(), claw->position());
}
if (back_.is_posedge()) {
// Saw a posedge on the hall effect. Reset the limits.
min_hall_effect_on_angle_ =
- ::std::min(claw.back.posedge_value, claw.position);
+ ::std::min(claw->back()->posedge_value(), claw->position());
max_hall_effect_on_angle_ =
- ::std::max(claw.back.posedge_value, claw.position);
+ ::std::max(claw->back()->posedge_value(), claw->position());
}
if (front_.is_negedge()) {
// Saw a negedge on the hall effect. Reset the limits.
min_hall_effect_off_angle_ =
- ::std::min(claw.front.negedge_value, claw.position);
+ ::std::min(claw->front()->negedge_value(), claw->position());
max_hall_effect_off_angle_ =
- ::std::max(claw.front.negedge_value, claw.position);
+ ::std::max(claw->front()->negedge_value(), claw->position());
}
if (calibration_.is_negedge()) {
// Saw a negedge on the hall effect. Reset the limits.
min_hall_effect_off_angle_ =
- ::std::min(claw.calibration.negedge_value, claw.position);
+ ::std::min(claw->calibration()->negedge_value(), claw->position());
max_hall_effect_off_angle_ =
- ::std::max(claw.calibration.negedge_value, claw.position);
+ ::std::max(claw->calibration()->negedge_value(), claw->position());
}
if (back_.is_negedge()) {
// Saw a negedge on the hall effect. Reset the limits.
min_hall_effect_off_angle_ =
- ::std::min(claw.back.negedge_value, claw.position);
+ ::std::min(claw->back()->negedge_value(), claw->position());
max_hall_effect_off_angle_ =
- ::std::max(claw.back.negedge_value, claw.position);
+ ::std::max(claw->back()->negedge_value(), claw->position());
}
last_encoder_ = encoder_;
@@ -292,23 +288,22 @@
} else {
last_off_encoder_ = encoder_;
}
- encoder_ = claw.position;
+ encoder_ = claw->position();
any_triggered_last_ = any_sensor_triggered;
}
-void ZeroedStateFeedbackLoop::Reset(
- const ::y2014::control_loops::HalfClawPosition &claw) {
+void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition *claw) {
set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- front_.Reset(claw.front);
- calibration_.Reset(claw.calibration);
- back_.Reset(claw.back);
+ front_.Reset(claw->front());
+ calibration_.Reset(claw->calibration());
+ back_.Reset(claw->back());
// close up the min and max edge positions as they are no longer valid and
// will be expanded in future iterations
- min_hall_effect_on_angle_ = claw.position;
- max_hall_effect_on_angle_ = claw.position;
- min_hall_effect_off_angle_ = claw.position;
- max_hall_effect_off_angle_ = claw.position;
+ min_hall_effect_on_angle_ = claw->position();
+ max_hall_effect_on_angle_ = claw->position();
+ min_hall_effect_off_angle_ = claw->position();
+ max_hall_effect_off_angle_ = claw->position();
any_triggered_last_ = any_triggered();
}
@@ -374,8 +369,8 @@
}
ClawMotor::ClawMotor(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<::y2014::control_loops::ClawQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -563,45 +558,33 @@
// first update position based on angle limit
const double separation = *top_goal - *bottom_goal;
if (separation > values.claw.soft_max_separation) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
const double dsep = (separation - values.claw.soft_max_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (separation < values.claw.soft_min_separation) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
const double dsep = (separation - values.claw.soft_min_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
// now move both goals in unison
if (*bottom_goal < values.claw.lower_claw.lower_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*top_goal += values.claw.lower_claw.lower_limit - *bottom_goal;
*bottom_goal = values.claw.lower_claw.lower_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*bottom_goal > values.claw.lower_claw.upper_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*top_goal -= *bottom_goal - values.claw.lower_claw.upper_limit;
*bottom_goal = values.claw.lower_claw.upper_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*top_goal < values.claw.upper_claw.lower_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*bottom_goal += values.claw.upper_claw.lower_limit - *top_goal;
*top_goal = values.claw.upper_claw.lower_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*top_goal > values.claw.upper_claw.upper_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*bottom_goal -= *top_goal - values.claw.upper_claw.upper_limit;
*top_goal = values.claw.upper_claw.upper_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
}
@@ -609,7 +592,7 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- ((has_joystick_state() ? joystick_state().autonomous : true) &&
+ ((has_joystick_state() ? joystick_state().autonomous() : true) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -621,43 +604,45 @@
bool ClawMotor::is_zeroing() const { return !is_ready(); }
// Positive angle is up, and positive power is up.
-void ClawMotor::RunIteration(
- const ::y2014::control_loops::ClawQueue::Goal *goal,
- const ::y2014::control_loops::ClawQueue::Position *position,
- ::y2014::control_loops::ClawQueue::Output *output,
- ::y2014::control_loops::ClawQueue::Status *status) {
+void ClawMotor::RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
// Disable the motors now so that all early returns will return with the
// motors disabled.
+ OutputT output_struct;
if (output) {
- output->top_claw_voltage = 0;
- output->bottom_claw_voltage = 0;
- output->intake_voltage = 0;
- output->tusk_voltage = 0;
+ output_struct.top_claw_voltage = 0;
+ output_struct.bottom_claw_voltage = 0;
+ output_struct.intake_voltage = 0;
+ output_struct.tusk_voltage = 0;
}
+ StatusT status_struct;
if (goal) {
- if (::std::isnan(goal->bottom_angle) ||
- ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
- ::std::isnan(goal->centering)) {
+ if (::std::isnan(goal->bottom_angle()) ||
+ ::std::isnan(goal->separation_angle()) ||
+ ::std::isnan(goal->intake()) || ::std::isnan(goal->centering())) {
+ status->Send(Status::Pack(*status->fbb(), &status_struct));
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
return;
}
}
if (WasReset()) {
- top_claw_.Reset(position->top);
- bottom_claw_.Reset(position->bottom);
+ top_claw_.Reset(position->top());
+ bottom_claw_.Reset(position->bottom());
}
const constants::Values &values = constants::GetValues();
if (position) {
Eigen::Matrix<double, 2, 1> Y;
- Y << position->bottom.position + bottom_claw_.offset(),
- position->top.position + top_claw_.offset();
+ Y << position->bottom()->position() + bottom_claw_.offset(),
+ position->top()->position() + top_claw_.offset();
claw_.Correct(Y);
- top_claw_.SetPositionValues(position->top);
- bottom_claw_.SetPositionValues(position->bottom);
+ top_claw_.SetPositionValues(position->top());
+ bottom_claw_.SetPositionValues(position->bottom());
if (!has_top_claw_goal_) {
has_top_claw_goal_ = true;
@@ -671,15 +656,12 @@
initial_separation_ =
top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
- AOS_LOG_STRUCT(DEBUG, "absolute position",
- ClawPositionToLog(top_claw_.absolute_position(),
- bottom_claw_.absolute_position()));
}
bool autonomous, enabled;
if (has_joystick_state()) {
- autonomous = joystick_state().autonomous;
- enabled = joystick_state().enabled;
+ autonomous = joystick_state().autonomous();
+ enabled = joystick_state().enabled();
} else {
autonomous = true;
enabled = false;
@@ -700,8 +682,8 @@
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
// Ready to use the claw.
// Limit the goals here.
- bottom_claw_goal_ = goal->bottom_angle;
- top_claw_goal_ = goal->bottom_angle + goal->separation_angle;
+ bottom_claw_goal_ = goal->bottom_angle();
+ top_claw_goal_ = goal->bottom_angle() + goal->separation_angle();
has_bottom_claw_goal_ = true;
has_top_claw_goal_ = true;
doing_calibration_fine_tune_ = false;
@@ -759,7 +741,7 @@
bottom_claw_.back())) {
// do calibration
bottom_claw_.SetCalibration(
- position->bottom.calibration.posedge_value,
+ position->bottom()->calibration()->posedge_value(),
values.claw.lower_claw.calibration.lower_angle);
bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning bottom
@@ -816,7 +798,7 @@
top_claw_.front(), top_claw_.back())) {
// do calibration
top_claw_.SetCalibration(
- position->top.calibration.posedge_value,
+ position->top()->calibration()->posedge_value(),
values.claw.upper_claw.calibration.lower_angle);
top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning top
@@ -836,10 +818,11 @@
doing_calibration_fine_tune_ = false;
if (!was_enabled_ && enabled) {
if (position) {
- top_claw_goal_ = position->top.position + top_claw_.offset();
- bottom_claw_goal_ = position->bottom.position + bottom_claw_.offset();
+ top_claw_goal_ = position->top()->position() + top_claw_.offset();
+ bottom_claw_goal_ =
+ position->bottom()->position() + bottom_claw_.offset();
initial_separation_ =
- position->top.position - position->bottom.position;
+ position->top()->position() - position->bottom()->position();
} else {
has_top_claw_goal_ = false;
has_bottom_claw_goal_ = false;
@@ -907,7 +890,6 @@
if (has_top_claw_goal_ && has_bottom_claw_goal_) {
claw_.mutable_R() << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
- AOS_LOG_MATRIX(DEBUG, "actual goal", claw_.R());
// Only cap power when one of the halves of the claw is moving slowly and
// could wind up.
@@ -973,55 +955,60 @@
if (output) {
if (goal) {
//setup the intake
- output->intake_voltage =
- (goal->intake > 12.0) ? 12 : (goal->intake < -12.0) ? -12.0
- : goal->intake;
- output->tusk_voltage = goal->centering;
- output->tusk_voltage =
- (goal->centering > 12.0) ? 12 : (goal->centering < -12.0)
+ output_struct.intake_voltage =
+ (goal->intake() > 12.0)
+ ? 12
+ : (goal->intake() < -12.0) ? -12.0 : goal->intake();
+ output_struct.tusk_voltage = goal->centering();
+ output_struct.tusk_voltage =
+ (goal->centering() > 12.0) ? 12 : (goal->centering() < -12.0)
? -12.0
- : goal->centering;
+ : goal->centering();
}
- output->top_claw_voltage = claw_.U(1, 0);
- output->bottom_claw_voltage = claw_.U(0, 0);
+ output_struct.top_claw_voltage = claw_.U(1, 0);
+ output_struct.bottom_claw_voltage = claw_.U(0, 0);
- if (output->top_claw_voltage > kMaxVoltage) {
- output->top_claw_voltage = kMaxVoltage;
- } else if (output->top_claw_voltage < -kMaxVoltage) {
- output->top_claw_voltage = -kMaxVoltage;
+ if (output_struct.top_claw_voltage > kMaxVoltage) {
+ output_struct.top_claw_voltage = kMaxVoltage;
+ } else if (output_struct.top_claw_voltage < -kMaxVoltage) {
+ output_struct.top_claw_voltage = -kMaxVoltage;
}
- if (output->bottom_claw_voltage > kMaxVoltage) {
- output->bottom_claw_voltage = kMaxVoltage;
- } else if (output->bottom_claw_voltage < -kMaxVoltage) {
- output->bottom_claw_voltage = -kMaxVoltage;
+ if (output_struct.bottom_claw_voltage > kMaxVoltage) {
+ output_struct.bottom_claw_voltage = kMaxVoltage;
+ } else if (output_struct.bottom_claw_voltage < -kMaxVoltage) {
+ output_struct.bottom_claw_voltage = -kMaxVoltage;
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
- status->bottom = bottom_absolute_position();
- status->separation = top_absolute_position() - bottom_absolute_position();
- status->bottom_velocity = claw_.X_hat(2, 0);
- status->separation_velocity = claw_.X_hat(3, 0);
+ status_struct.bottom = bottom_absolute_position();
+ status_struct.separation =
+ top_absolute_position() - bottom_absolute_position();
+ status_struct.bottom_velocity = claw_.X_hat(2, 0);
+ status_struct.separation_velocity = claw_.X_hat(3, 0);
if (goal) {
bool bottom_done =
- ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
- bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle()) < 0.020;
+ bool bottom_velocity_done = ::std::abs(status_struct.bottom_velocity) < 0.2;
bool separation_done =
::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.020;
+ goal->separation_angle()) < 0.020;
bool separation_done_with_ball =
::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.06;
- status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
- status->done_with_ball =
- is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ goal->separation_angle()) < 0.06;
+ status_struct.done =
+ is_ready() && separation_done && bottom_done && bottom_velocity_done;
+ status_struct.done_with_ball = is_ready() && separation_done_with_ball &&
+ bottom_done && bottom_velocity_done;
} else {
- status->done = status->done_with_ball = false;
+ status_struct.done = status_struct.done_with_ball = false;
}
- status->zeroed = is_ready();
- status->zeroed_for_auto =
+ status_struct.zeroed = is_ready();
+ status_struct.zeroed_for_auto =
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -1029,8 +1016,11 @@
bottom_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ status->Send(Status::Pack(*status->fbb(), &status_struct));
+
was_enabled_ = enabled;
}
+} // namespace claw
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/claw/claw.h b/y2014/control_loops/claw/claw.h
index 082ddce..5bcccc1 100644
--- a/y2014/control_loops/claw/claw.h
+++ b/y2014/control_loops/claw/claw.h
@@ -5,15 +5,19 @@
#include "aos/controls/control_loop.h"
#include "aos/controls/polytope.h"
-#include "y2014/constants.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/claw/claw_motor_plant.h"
#include "frc971/control_loops/hall_effect_tracker.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_motor_plant.h"
+#include "y2014/control_loops/claw/claw_output_generated.h"
+#include "y2014/control_loops/claw/claw_position_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
namespace y2014 {
namespace control_loops {
+namespace claw {
namespace testing {
class WindupClawTest;
};
@@ -77,9 +81,9 @@
}
JointZeroingState zeroing_state() const { return zeroing_state_; }
- void SetPositionValues(const ::y2014::control_loops::HalfClawPosition &claw);
+ void SetPositionValues(const HalfClawPosition *claw);
- void Reset(const ::y2014::control_loops::HalfClawPosition &claw);
+ void Reset(const HalfClawPosition *claw);
double absolute_position() const { return encoder() + offset(); }
@@ -183,11 +187,10 @@
};
class ClawMotor
- : public aos::controls::ControlLoop<::y2014::control_loops::ClawQueue> {
+ : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit ClawMotor(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2014.control_loops.claw_queue");
+ explicit ClawMotor(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/claw");
// True if the state machine is ready.
bool capped_goal() const { return capped_goal_; }
@@ -217,11 +220,9 @@
CalibrationMode mode() const { return mode_; }
protected:
- virtual void RunIteration(
- const ::y2014::control_loops::ClawQueue::Goal *goal,
- const ::y2014::control_loops::ClawQueue::Position *position,
- ::y2014::control_loops::ClawQueue::Output *output,
- ::y2014::control_loops::ClawQueue::Status *status);
+ virtual void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status);
double top_absolute_position() const {
return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
@@ -262,6 +263,7 @@
void LimitClawGoal(double *bottom_goal, double *top_goal,
const constants::Values &values);
+} // namespace claw
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/claw/claw.q b/y2014/control_loops/claw/claw.q
deleted file mode 100644
index ff4c8b8..0000000
--- a/y2014/control_loops/claw/claw.q
+++ /dev/null
@@ -1,74 +0,0 @@
-package y2014.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct HalfClawPosition {
- // The current position of this half of the claw.
- double position;
-
- // The hall effect sensor at the front limit.
- .frc971.HallEffectStruct front;
- // The hall effect sensor in the middle to use for real calibration.
- .frc971.HallEffectStruct calibration;
- // The hall effect at the back limit.
- .frc971.HallEffectStruct back;
-};
-
-// All angles here are 0 vertical, positive "up" (aka backwards).
-// Published on ".y2014.control_loops.claw_queue"
-queue_group ClawQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // The angle of the bottom claw.
- double bottom_angle;
- // How much higher the top claw is.
- double separation_angle;
- // top claw intake roller
- double intake;
- // bottom claw tusk centering
- double centering;
- };
-
- message Position {
- // All the top claw information.
- HalfClawPosition top;
- // All the bottom claw information.
- HalfClawPosition bottom;
- };
-
- message Output {
- double intake_voltage;
- double top_claw_voltage;
- double bottom_claw_voltage;
- double tusk_voltage;
- };
-
- message Status {
- // True if zeroed enough for the current period (autonomous or teleop).
- bool zeroed;
- // True if zeroed as much as we will force during autonomous.
- bool zeroed_for_auto;
- // True if zeroed and within tolerance for separation and bottom angle.
- bool done;
- // True if zeroed and within tolerance for separation and bottom angle.
- // seperation allowance much wider as a ball may be included
- bool done_with_ball;
- // Dump the values of the state matrix.
- double bottom;
- double bottom_velocity;
- double separation;
- double separation_velocity;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
-
-struct ClawPositionToLog {
- double top;
- double bottom;
-};
diff --git a/y2014/control_loops/claw/claw_goal.fbs b/y2014/control_loops/claw/claw_goal.fbs
new file mode 100644
index 0000000..eb5860c
--- /dev/null
+++ b/y2014/control_loops/claw/claw_goal.fbs
@@ -0,0 +1,15 @@
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table Goal {
+ // The angle of the bottom claw.
+ bottom_angle:double;
+ // How much higher the top claw is.
+ separation_angle:double;
+ // top claw intake roller
+ intake:double;
+ // bottom claw tusk centering
+ centering:double;
+}
+
+root_type Goal;
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index 41dbfca..55e186e 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -8,16 +8,18 @@
#include "gtest/gtest.h"
#include "y2014/constants.h"
#include "y2014/control_loops/claw/claw.h"
-#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
#include "y2014/control_loops/claw/claw_motor_plant.h"
+#include "y2014/control_loops/claw/claw_output_generated.h"
+#include "y2014/control_loops/claw/claw_position_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
namespace y2014 {
namespace control_loops {
+namespace claw {
namespace testing {
-using ::y2014::control_loops::claw::MakeClawPlant;
-using ::frc971::HallEffectStruct;
-using ::y2014::control_loops::HalfClawPosition;
+using ::frc971::HallEffectStructT;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -38,10 +40,8 @@
double initial_top_position,
double initial_bottom_position)
: event_loop_(event_loop),
- claw_position_sender_(event_loop_->MakeSender<ClawQueue::Position>(
- ".y2014.control_loops.claw_queue.position")),
- claw_output_fetcher_(event_loop_->MakeFetcher<ClawQueue::Output>(
- ".y2014.control_loops.claw_queue.output")),
+ claw_position_sender_(event_loop_->MakeSender<Position>("/claw")),
+ claw_output_fetcher_(event_loop_->MakeFetcher<Output>("/claw")),
claw_plant_(new StateFeedbackPlant<4, 2, 2>(MakeClawPlant())) {
Reinitialize(initial_top_position, initial_bottom_position);
@@ -69,7 +69,9 @@
ReinitializePartial(TOP_CLAW, initial_top_position);
ReinitializePartial(BOTTOM_CLAW, initial_bottom_position);
- last_position_.Zero();
+
+ last_position_.top.reset();
+ last_position_.bottom.reset();
SetPhysicalSensors(&last_position_);
}
@@ -99,44 +101,58 @@
void SetHallEffect(double pos,
const constants::Values::Claws::AnglePair &pair,
- HallEffectStruct *hall_effect) {
+ HallEffectStructT *hall_effect) {
hall_effect->current = CheckRange(pos, pair);
}
void SetClawHallEffects(double pos,
const constants::Values::Claws::Claw &claw,
- HalfClawPosition *half_claw) {
- SetHallEffect(pos, claw.front, &half_claw->front);
- SetHallEffect(pos, claw.calibration, &half_claw->calibration);
- SetHallEffect(pos, claw.back, &half_claw->back);
+ HalfClawPositionT *half_claw) {
+ if (!half_claw->front) {
+ half_claw->front.reset(new HallEffectStructT());
+ }
+ if (!half_claw->calibration) {
+ half_claw->calibration.reset(new HallEffectStructT());
+ }
+ if (!half_claw->back) {
+ half_claw->back.reset(new HallEffectStructT());
+ }
+ SetHallEffect(pos, claw.front, half_claw->front.get());
+ SetHallEffect(pos, claw.calibration, half_claw->calibration.get());
+ SetHallEffect(pos, claw.back, half_claw->back.get());
}
// Sets the values of the physical sensors that can be directly observed
// (encoder, hall effect).
- void SetPhysicalSensors(
- ::y2014::control_loops::ClawQueue::Position *position) {
- position->top.position = GetPosition(TOP_CLAW);
- position->bottom.position = GetPosition(BOTTOM_CLAW);
+ void SetPhysicalSensors(PositionT *position) {
+ if (!position->top) {
+ position->top.reset(new HalfClawPositionT());
+ }
+ if (!position->bottom) {
+ position->bottom.reset(new HalfClawPositionT());
+ }
+
+ position->top->position = GetPosition(TOP_CLAW);
+ position->bottom->position = GetPosition(BOTTOM_CLAW);
double pos[2] = {GetAbsolutePosition(TOP_CLAW),
GetAbsolutePosition(BOTTOM_CLAW)};
AOS_LOG(DEBUG, "Physical claws are at {top: %f, bottom: %f}\n",
pos[TOP_CLAW], pos[BOTTOM_CLAW]);
- const constants::Values& values = constants::GetValues();
+ const constants::Values &values = constants::GetValues();
// Signal that each hall effect sensor has been triggered if it is within
// the correct range.
- SetClawHallEffects(pos[TOP_CLAW], values.claw.upper_claw, &position->top);
+ SetClawHallEffects(pos[TOP_CLAW], values.claw.upper_claw,
+ position->top.get());
SetClawHallEffects(pos[BOTTOM_CLAW], values.claw.lower_claw,
- &position->bottom);
+ position->bottom.get());
}
- void UpdateHallEffect(double angle,
- double last_angle,
- double initial_position,
- HallEffectStruct *position,
- const HallEffectStruct &last_position,
+ void UpdateHallEffect(double angle, double last_angle,
+ double initial_position, HallEffectStructT *position,
+ const HallEffectStructT &last_position,
const constants::Values::Claws::AnglePair &pair,
const char *claw_name, const char *hall_effect_name) {
if (position->current && !last_position.current) {
@@ -167,49 +183,64 @@
}
}
- void UpdateClawHallEffects(
- HalfClawPosition *position,
- const HalfClawPosition &last_position,
- const constants::Values::Claws::Claw &claw, double initial_position,
- const char *claw_name) {
- UpdateHallEffect(position->position + initial_position,
+ void UpdateClawHallEffects(HalfClawPositionT *half_claw,
+ const HalfClawPositionT &last_position,
+ const constants::Values::Claws::Claw &claw,
+ double initial_position, const char *claw_name) {
+ if (!half_claw->front) {
+ half_claw->front.reset(new HallEffectStructT());
+ }
+ if (!half_claw->calibration) {
+ half_claw->calibration.reset(new HallEffectStructT());
+ }
+ if (!half_claw->back) {
+ half_claw->back.reset(new HallEffectStructT());
+ }
+ UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->front, last_position.front,
- claw.front, claw_name, "front");
- UpdateHallEffect(position->position + initial_position,
+ initial_position, half_claw->front.get(),
+ *last_position.front.get(), claw.front, claw_name, "front");
+ UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->calibration,
- last_position.calibration, claw.calibration, claw_name,
- "calibration");
- UpdateHallEffect(position->position + initial_position,
+ initial_position, half_claw->calibration.get(),
+ *last_position.calibration.get(), claw.calibration,
+ claw_name, "calibration");
+ UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->back, last_position.back,
- claw.back, claw_name, "back");
+ initial_position, half_claw->back.get(),
+ *last_position.back.get(), claw.back, claw_name, "back");
}
// Sends out the position queue messages.
void SendPositionMessage() {
- ::aos::Sender<::y2014::control_loops::ClawQueue::Position>::Message
- position = claw_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ claw_position_sender_.MakeBuilder();
// Initialize all the counters to their previous values.
- *position = last_position_;
- SetPhysicalSensors(position.get());
+ PositionT position;
- const constants::Values& values = constants::GetValues();
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<Position> position_offset =
+ Position::Pack(fbb, &last_position_);
+ fbb.Finish(position_offset);
- UpdateClawHallEffects(&position->top, last_position_.top,
+ flatbuffers::GetRoot<Position>(fbb.GetBufferPointer())->UnPackTo(&position);
+ SetPhysicalSensors(&position);
+
+ const constants::Values &values = constants::GetValues();
+
+ UpdateClawHallEffects(position.top.get(), *last_position_.top.get(),
values.claw.upper_claw, initial_position_[TOP_CLAW],
"Top");
- UpdateClawHallEffects(&position->bottom, last_position_.bottom,
+ UpdateClawHallEffects(position.bottom.get(), *last_position_.bottom.get(),
values.claw.lower_claw,
initial_position_[BOTTOM_CLAW], "Bottom");
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- last_position_ = *position;
- position.Send();
+ builder.Send(Position::Pack(*builder.fbb(), &position));
+ last_position_ = std::move(position);
}
// Simulates the claw moving for one timestep.
@@ -218,8 +249,8 @@
EXPECT_TRUE(claw_output_fetcher_.Fetch());
Eigen::Matrix<double, 2, 1> U;
- U << claw_output_fetcher_->bottom_claw_voltage,
- claw_output_fetcher_->top_claw_voltage;
+ U << claw_output_fetcher_->bottom_claw_voltage(),
+ claw_output_fetcher_->top_claw_voltage();
claw_plant_->Update(U);
// Check that the claw is within the limits.
@@ -237,8 +268,8 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<ClawQueue::Position> claw_position_sender_;
- ::aos::Fetcher<ClawQueue::Output> claw_output_fetcher_;
+ ::aos::Sender<Position> claw_position_sender_;
+ ::aos::Fetcher<Output> claw_output_fetcher_;
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -252,18 +283,18 @@
double initial_position_[CLAW_COUNT];
- ::y2014::control_loops::ClawQueue::Position last_position_;
+ PositionT last_position_;
};
class ClawTest : public ::aos::testing::ControlLoopTest {
protected:
ClawTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2014/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
- claw_goal_sender_(test_event_loop_->MakeSender<ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
- claw_goal_fetcher_(test_event_loop_->MakeFetcher<ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ claw_goal_sender_(test_event_loop_->MakeSender<Goal>("/claw")),
+ claw_goal_fetcher_(test_event_loop_->MakeFetcher<Goal>("/claw")),
claw_event_loop_(MakeEventLoop()),
claw_motor_(claw_event_loop_.get()),
claw_plant_event_loop_(MakeEventLoop()),
@@ -275,14 +306,14 @@
double bottom = claw_motor_plant_.GetAbsolutePosition(BOTTOM_CLAW);
double separation =
claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
- EXPECT_NEAR(claw_goal_fetcher_->bottom_angle, bottom, 1e-4);
- EXPECT_NEAR(claw_goal_fetcher_->separation_angle, separation, 1e-4);
+ EXPECT_NEAR(claw_goal_fetcher_->bottom_angle(), bottom, 1e-4);
+ EXPECT_NEAR(claw_goal_fetcher_->separation_angle(), separation, 1e-4);
EXPECT_LE(min_separation_, separation);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Sender<ClawQueue::Goal> claw_goal_sender_;
- ::aos::Fetcher<ClawQueue::Goal> claw_goal_fetcher_;
+ ::aos::Sender<Goal> claw_goal_sender_;
+ ::aos::Fetcher<Goal> claw_goal_fetcher_;
// Create a loop and simulation plant.
::std::unique_ptr<::aos::EventLoop> claw_event_loop_;
@@ -294,15 +325,15 @@
// Minimum amount of acceptable separation between the top and bottom of the
// claw.
double min_separation_;
-
};
TEST_F(ClawTest, HandlesNAN) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = ::std::nan("");
- message->separation_angle = ::std::nan("");
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(::std::nan(""));
+ goal_builder.add_separation_angle(::std::nan(""));
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(true);
@@ -312,10 +343,11 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ClawTest, ZerosCorrectly) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(true);
@@ -409,10 +441,11 @@
claw_motor_plant_.Reinitialize(GetParam().first, GetParam().second);
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(true);
@@ -586,10 +619,11 @@
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupPositive) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
TestWindup(ClawMotor::UNKNOWN_LOCATION,
@@ -602,10 +636,11 @@
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupNegative) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
TestWindup(ClawMotor::UNKNOWN_LOCATION,
@@ -618,10 +653,11 @@
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupNegativeFineTune) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
TestWindup(ClawMotor::FINE_TUNE_BOTTOM,
@@ -631,5 +667,6 @@
}
} // namespace testing
+} // namespace claw
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/claw/claw_main.cc b/y2014/control_loops/claw/claw_main.cc
index 65627d5..f21c7c5 100644
--- a/y2014/control_loops/claw/claw_main.cc
+++ b/y2014/control_loops/claw/claw_main.cc
@@ -1,13 +1,16 @@
#include "y2014/control_loops/claw/claw.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
- ::y2014::control_loops::ClawMotor claw(&event_loop);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2014::control_loops::claw::ClawMotor claw(&event_loop);
event_loop.Run();
diff --git a/y2014/control_loops/claw/claw_output.fbs b/y2014/control_loops/claw/claw_output.fbs
new file mode 100644
index 0000000..8cb42e0
--- /dev/null
+++ b/y2014/control_loops/claw/claw_output.fbs
@@ -0,0 +1,11 @@
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table Output {
+ intake_voltage:double;
+ top_claw_voltage:double;
+ bottom_claw_voltage:double;
+ tusk_voltage:double;
+}
+
+root_type Output;
diff --git a/y2014/control_loops/claw/claw_position.fbs b/y2014/control_loops/claw/claw_position.fbs
new file mode 100644
index 0000000..17d9f17
--- /dev/null
+++ b/y2014/control_loops/claw/claw_position.fbs
@@ -0,0 +1,25 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table HalfClawPosition {
+ // The current position of this half of the claw.
+ position:double;
+
+ // The hall effect sensor at the front limit.
+ front:frc971.HallEffectStruct;
+ // The hall effect sensor in the middle to use for real calibration.
+ calibration:frc971.HallEffectStruct;
+ // The hall effect at the back limit.
+ back:frc971.HallEffectStruct;
+}
+
+table Position {
+ // All the top claw information.
+ top:HalfClawPosition;
+ // All the bottom claw information.
+ bottom:HalfClawPosition;
+}
+
+root_type Position;
diff --git a/y2014/control_loops/claw/claw_status.fbs b/y2014/control_loops/claw/claw_status.fbs
new file mode 100644
index 0000000..eda9279
--- /dev/null
+++ b/y2014/control_loops/claw/claw_status.fbs
@@ -0,0 +1,21 @@
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table Status {
+ // True if zeroed enough for the current period (autonomous or teleop).
+ zeroed:bool;
+ // True if zeroed as much as we will force during autonomous.
+ zeroed_for_auto:bool;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ done:bool;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ // seperation allowance much wider as a ball may be included
+ done_with_ball:bool;
+ // Dump the values of the state matrix.
+ bottom:double;
+ bottom_velocity:double;
+ separation:double;
+ separation_velocity:double;
+}
+
+root_type Status;
diff --git a/y2014/control_loops/drivetrain/BUILD b/y2014/control_loops/drivetrain/BUILD
index 858f7e0..c458208 100644
--- a/y2014/control_loops/drivetrain/BUILD
+++ b/y2014/control_loops/drivetrain/BUILD
@@ -1,7 +1,5 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
@@ -77,7 +75,7 @@
deps = [
":drivetrain_base",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2014/control_loops/drivetrain/drivetrain_main.cc b/y2014/control_loops/drivetrain/drivetrain_main.cc
index 7749631..c210770 100644
--- a/y2014/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_main.cc
@@ -1,6 +1,6 @@
#include "aos/init.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2014/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,10 @@
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop, ::y2014::control_loops::GetDrivetrainConfig());
DrivetrainLoop drivetrain(::y2014::control_loops::GetDrivetrainConfig(),
diff --git a/y2014/control_loops/shooter/BUILD b/y2014/control_loops/shooter/BUILD
index 075cafc..a2073c9 100644
--- a/y2014/control_loops/shooter/BUILD
+++ b/y2014/control_loops/shooter/BUILD
@@ -1,16 +1,40 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "shooter_queue",
+flatbuffer_cc_library(
+ name = "shooter_goal_fbs",
srcs = [
- "shooter.q",
+ "shooter_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_position_fbs",
+ srcs = [
+ "shooter_position.fbs",
],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "shooter_output_fbs",
+ srcs = [
+ "shooter_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_status_fbs",
+ srcs = [
+ "shooter_status.fbs",
+ ],
+ gen_reflections = 1,
)
genrule(
@@ -44,9 +68,12 @@
"-lm",
],
deps = [
- ":shooter_queue",
+ ":shooter_goal_fbs",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop",
- "//aos/logging:queue_logging",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
"//y2014:constants",
],
@@ -57,9 +84,13 @@
srcs = [
"shooter_lib_test.cc",
],
+ data = ["//y2014:config.json"],
deps = [
+ ":shooter_goal_fbs",
":shooter_lib",
- ":shooter_queue",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
@@ -75,6 +106,6 @@
deps = [
":shooter_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index bf14aad..fb115c7 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -6,15 +6,13 @@
#include <limits>
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-
#include "y2014/constants.h"
#include "y2014/control_loops/shooter/shooter_motor_plant.h"
namespace y2014 {
namespace control_loops {
+namespace shooter {
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
@@ -45,10 +43,6 @@
voltage_ = std::max(-max_voltage_, voltage_);
mutable_U(0, 0) = voltage_ - old_voltage;
- AOS_LOG_STRUCT(
- DEBUG, "shooter_output",
- ::y2014::control_loops::ShooterVoltageToLog(X_hat(2, 0), voltage_));
-
last_voltage_ = voltage_;
capped_goal_ = false;
}
@@ -67,8 +61,6 @@
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
- AOS_LOG_STRUCT(DEBUG, "to prevent windup",
- ::y2014::control_loops::ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
if (index() == 0) {
@@ -82,8 +74,6 @@
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
- AOS_LOG_STRUCT(DEBUG, "to prevent windup",
- ::y2014::control_loops::ShooterMovingGoal(dx));
} else {
capped_goal_ = false;
}
@@ -100,7 +90,6 @@
void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
double known_position) {
- double old_position = absolute_position();
double previous_offset = offset_;
offset_ = known_position - encoder_val;
double doffset = offset_ - previous_offset;
@@ -110,16 +99,12 @@
if (index() == 0) {
mutable_R(2, 0) += -plant().A(1, 0) / plant().A(1, 2) * (doffset);
}
- AOS_LOG_STRUCT(DEBUG, "sensor edge (fake?)",
- ::y2014::control_loops::ShooterChangeCalibration(
- encoder_val, known_position, old_position,
- absolute_position(), previous_offset, offset_));
}
ShooterMotor::ShooterMotor(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue>(
- event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
shooter_(MakeShooterLoop()),
state_(STATE_INITIALIZE),
cycles_not_moved_(0),
@@ -137,12 +122,8 @@
(kMaxExtension - values.shooter.upper_limit) *
(kMaxExtension - values.shooter.upper_limit));
if (power < 0) {
- AOS_LOG_STRUCT(WARNING, "negative power",
- ::y2014::control_loops::PowerAdjustment(power, 0));
power = 0;
} else if (power > maxpower) {
- AOS_LOG_STRUCT(WARNING, "power too high",
- ::y2014::control_loops::PowerAdjustment(power, maxpower));
power = maxpower;
}
@@ -166,23 +147,23 @@
return power;
}
-void ShooterMotor::CheckCalibrations(
- const ::y2014::control_loops::ShooterQueue::Position *position) {
+void ShooterMotor::CheckCalibrations(const Position *position) {
AOS_CHECK_NOTNULL(position);
const constants::Values &values = constants::GetValues();
// TODO(austin): Validate that this is the right edge.
// If we see a posedge on any of the hall effects,
- if (position->pusher_proximal.posedge_count != last_proximal_posedge_count_ &&
+ if (position->pusher_proximal()->posedge_count() !=
+ last_proximal_posedge_count_ &&
!last_proximal_current_) {
proximal_posedge_validation_cycles_left_ = 2;
}
if (proximal_posedge_validation_cycles_left_ > 0) {
- if (position->pusher_proximal.current) {
+ if (position->pusher_proximal()->current()) {
--proximal_posedge_validation_cycles_left_;
if (proximal_posedge_validation_cycles_left_ == 0) {
shooter_.SetCalibration(
- position->pusher_proximal.posedge_value,
+ position->pusher_proximal()->posedge_value(),
values.shooter.pusher_proximal.upper_angle);
AOS_LOG(DEBUG, "Setting calibration using proximal sensor\n");
@@ -193,16 +174,17 @@
}
}
- if (position->pusher_distal.posedge_count != last_distal_posedge_count_ &&
+ if (position->pusher_distal()->posedge_count() !=
+ last_distal_posedge_count_ &&
!last_distal_current_) {
distal_posedge_validation_cycles_left_ = 2;
}
if (distal_posedge_validation_cycles_left_ > 0) {
- if (position->pusher_distal.current) {
+ if (position->pusher_distal()->current()) {
--distal_posedge_validation_cycles_left_;
if (distal_posedge_validation_cycles_left_ == 0) {
shooter_.SetCalibration(
- position->pusher_distal.posedge_value,
+ position->pusher_distal()->posedge_value(),
values.shooter.pusher_distal.upper_angle);
AOS_LOG(DEBUG, "Setting calibration using distal sensor\n");
@@ -216,43 +198,44 @@
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
- const ::y2014::control_loops::ShooterQueue::Goal *goal,
- const ::y2014::control_loops::ShooterQueue::Position *position,
- ::y2014::control_loops::ShooterQueue::Output *output,
- ::y2014::control_loops::ShooterQueue::Status *status) {
+ const Goal *goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
+ OutputT output_struct;
const monotonic_clock::time_point monotonic_now = event_loop()->monotonic_now();
- if (goal && ::std::isnan(goal->shot_power)) {
+ if (goal && ::std::isnan(goal->shot_power())) {
state_ = STATE_ESTOP;
AOS_LOG(ERROR, "Estopping because got a shot power of NAN.\n");
}
// we must always have these or we have issues.
if (status == NULL) {
- if (output) output->voltage = 0;
+ if (output) output_struct.voltage = 0;
AOS_LOG(ERROR, "Thought I would just check for null and die.\n");
return;
}
- status->ready = false;
+ bool status_ready = false;
if (WasReset()) {
state_ = STATE_INITIALIZE;
- last_distal_current_ = position->pusher_distal.current;
- last_proximal_current_ = position->pusher_proximal.current;
+ last_distal_current_ = position->pusher_distal()->current();
+ last_proximal_current_ = position->pusher_proximal()->current();
}
if (position) {
- shooter_.CorrectPosition(position->position);
+ shooter_.CorrectPosition(position->position());
}
// Disable the motors now so that all early returns will return with the
// motors disabled.
- if (output) output->voltage = 0;
+ if (output) output_struct.voltage = 0;
const constants::Values &values = constants::GetValues();
// Don't even let the control loops run.
bool shooter_loop_disable = false;
- const bool disabled = !has_joystick_state() || !joystick_state().enabled;
+ const bool disabled = !has_joystick_state() || !joystick_state().enabled();
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -263,7 +246,7 @@
if (position) {
int last_index = shooter_.index();
- if (position->plunger && position->latch) {
+ if (position->plunger() && position->latch()) {
// Use the controller without the spring if the latch is set and the
// plunger is back
shooter_.set_index(1);
@@ -280,27 +263,27 @@
case STATE_INITIALIZE:
if (position) {
// Reinitialize the internal filter state.
- shooter_.InitializeState(position->position);
+ shooter_.InitializeState(position->position());
// Start off with the assumption that we are at the value
// futhest back given our sensors.
- if (position->pusher_distal.current) {
- shooter_.SetCalibration(position->position,
+ if (position->pusher_distal()->current()) {
+ shooter_.SetCalibration(position->position(),
values.shooter.pusher_distal.lower_angle);
- } else if (position->pusher_proximal.current) {
- shooter_.SetCalibration(position->position,
+ } else if (position->pusher_proximal()->current()) {
+ shooter_.SetCalibration(position->position(),
values.shooter.pusher_proximal.upper_angle);
} else {
- shooter_.SetCalibration(position->position,
+ shooter_.SetCalibration(position->position(),
values.shooter.upper_limit);
}
// Go to the current position.
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
// If the plunger is all the way back, we want to be latched.
- latch_piston_ = position->plunger;
+ latch_piston_ = position->plunger();
brake_piston_ = false;
- if (position->latch == latch_piston_) {
+ if (position->latch() == latch_piston_) {
state_ = STATE_REQUEST_LOAD;
} else {
shooter_loop_disable = true;
@@ -317,20 +300,20 @@
case STATE_REQUEST_LOAD:
if (position) {
zeroed_ = false;
- if (position->pusher_distal.current ||
- position->pusher_proximal.current) {
+ if (position->pusher_distal()->current() ||
+ position->pusher_proximal()->current()) {
// We started on the sensor, back up until we are found.
// If the plunger is all the way back and not latched, it won't be
// there for long.
state_ = STATE_LOAD_BACKTRACK;
// The plunger is already back and latched. Don't release it.
- if (position->plunger && position->latch) {
+ if (position->plunger() && position->latch()) {
latch_piston_ = true;
} else {
latch_piston_ = false;
}
- } else if (position->plunger && position->latch) {
+ } else if (position->plunger() && position->latch()) {
// The plunger is back and we are latched. We most likely got here
// from Initialize, in which case we want to 'load' again anyways to
// zero.
@@ -361,11 +344,11 @@
shooter_.set_max_voltage(4.0);
if (position) {
- if (!position->pusher_distal.current &&
- !position->pusher_proximal.current) {
+ if (!position->pusher_distal()->current() &&
+ !position->pusher_proximal()->current()) {
Load(monotonic_now);
}
- latch_piston_ = position->plunger;
+ latch_piston_ = position->plunger();
}
brake_piston_ = false;
@@ -391,18 +374,19 @@
// Latch if the plunger is far enough back to trigger the hall effect.
// This happens when the distal sensor is triggered.
- latch_piston_ = position->pusher_distal.current || position->plunger;
+ latch_piston_ =
+ position->pusher_distal()->current() || position->plunger();
// Check if we are latched and back. Make sure the plunger is all the
// way back as well.
- if (position->plunger && position->latch &&
- position->pusher_distal.current) {
+ if (position->plunger() && position->latch() &&
+ position->pusher_distal()->current()) {
if (!zeroed_) {
state_ = STATE_REQUEST_LOAD;
} else {
state_ = STATE_PREPARE_SHOT;
}
- } else if (position->plunger &&
+ } else if (position->plunger() &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
// We are at the goal, but not latched.
@@ -415,10 +399,9 @@
if (position) {
// If none of the sensors is triggered, estop.
// Otherwise, trigger anyways if it has been 0.5 seconds more.
- if (!(position->pusher_distal.current ||
- position->pusher_proximal.current) ||
- (load_timeout_ + chrono::milliseconds(500) <
- monotonic_now)) {
+ if (!(position->pusher_distal()->current() ||
+ position->pusher_proximal()->current()) ||
+ (load_timeout_ + chrono::milliseconds(500) < monotonic_now)) {
state_ = STATE_ESTOP;
AOS_LOG(ERROR, "Estopping because took too long to load.\n");
}
@@ -436,7 +419,7 @@
if (monotonic_now > loading_problem_end_time_) {
// Timeout by unloading.
Unload(monotonic_now);
- } else if (position && position->plunger && position->latch) {
+ } else if (position && position->plunger() && position->latch()) {
// If both trigger, we are latched.
state_ = STATE_PREPARE_SHOT;
}
@@ -448,7 +431,7 @@
shooter_.SetGoalPosition(values.shooter.lower_limit, 0.0);
if (position) {
AOS_LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
- position->plunger, position->latch);
+ position->plunger(), position->latch());
}
latch_piston_ = true;
@@ -459,16 +442,16 @@
// TODO(austin): Timeout. Low priority.
if (goal) {
- shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power()), 0.0);
}
AOS_LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
shooter_.absolute_position(),
- goal ? PowerToPosition(goal->shot_power)
+ goal ? PowerToPosition(goal->shot_power())
: ::std::numeric_limits<double>::quiet_NaN());
if (goal &&
::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) < 0.001 &&
+ PowerToPosition(goal->shot_power())) < 0.001 &&
::std::abs(shooter_.absolute_velocity()) < 0.005) {
// We are there, set the brake and move on.
latch_piston_ = true;
@@ -479,7 +462,7 @@
latch_piston_ = true;
brake_piston_ = false;
}
- if (goal && goal->unload_requested) {
+ if (goal && goal->unload_requested()) {
Unload(monotonic_now);
}
break;
@@ -488,12 +471,12 @@
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
if (monotonic_now > shooter_brake_set_time_) {
- status->ready = true;
+ status_ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
shooter_loop_disable = true;
AOS_LOG(DEBUG, "Brake is now set\n");
- if (goal && goal->shot_requested && !disabled) {
+ if (goal && goal->shot_requested() && !disabled) {
AOS_LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
shot_end_time_ = monotonic_now + kShotEndTimeout;
@@ -503,24 +486,24 @@
}
if (state_ == STATE_READY && goal &&
::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) > 0.002) {
+ PowerToPosition(goal->shot_power())) > 0.002) {
// TODO(austin): Add a state to release the brake.
// TODO(austin): Do we want to set the brake here or after shooting?
// Depends on air usage.
- status->ready = false;
+ status_ready = false;
AOS_LOG(DEBUG, "Preparing shot again.\n");
state_ = STATE_PREPARE_SHOT;
}
if (goal) {
- shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power()), 0.0);
}
latch_piston_ = true;
brake_piston_ = true;
- if (goal && goal->unload_requested) {
+ if (goal && goal->unload_requested()) {
Unload(monotonic_now);
}
break;
@@ -528,7 +511,7 @@
case STATE_FIRE:
if (disabled) {
if (position) {
- if (position->plunger) {
+ if (position->plunger()) {
// If disabled and the plunger is still back there, reset the
// timeout.
shot_end_time_ = monotonic_now + kShotEndTimeout;
@@ -537,7 +520,7 @@
}
shooter_loop_disable = true;
// Count the number of contiguous cycles during which we haven't moved.
- if (::std::abs(last_position_.position - shooter_.absolute_position()) <
+ if (::std::abs(last_position_position_ - shooter_.absolute_position()) <
0.0005) {
++cycles_not_moved_;
} else {
@@ -551,7 +534,7 @@
shooter_.absolute_position()) > 0.0005 &&
cycles_not_moved_ > 6) ||
monotonic_now > shot_end_time_) &&
- robot_state().voltage_battery > 10.5) {
+ robot_state().voltage_battery() > 10.5) {
state_ = STATE_REQUEST_LOAD;
++shot_count_;
}
@@ -566,9 +549,9 @@
// the plunger.
bool all_back;
if (position) {
- all_back = position->plunger && position->latch;
+ all_back = position->plunger() && position->latch();
} else {
- all_back = last_position_.plunger && last_position_.latch;
+ all_back = last_position_plunger_ && last_position_latch_;
}
if (all_back) {
@@ -632,7 +615,7 @@
brake_piston_ = false;
} break;
case STATE_READY_UNLOAD:
- if (goal && goal->load_requested) {
+ if (goal && goal->load_requested()) {
state_ = STATE_REQUEST_LOAD;
}
// If we are ready to load again,
@@ -652,9 +635,6 @@
}
if (!shooter_loop_disable) {
- AOS_LOG_STRUCT(DEBUG, "running the loop",
- ::y2014::control_loops::ShooterStatusToLog(
- shooter_.goal_position(), shooter_.absolute_position()));
if (!cap_goal) {
shooter_.set_max_voltage(12.0);
}
@@ -664,49 +644,55 @@
}
// We don't really want to output anything if we went through everything
// assuming the motors weren't working.
- if (output) output->voltage = shooter_.voltage();
+ if (output) output_struct.voltage = shooter_.voltage();
} else {
shooter_.Update(true);
shooter_.ZeroPower();
- if (output) output->voltage = 0.0;
+ if (output) output_struct.voltage = 0.0;
}
- status->hard_stop_power = PositionToPower(shooter_.absolute_position());
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_ready(status_ready);
+ status_builder.add_hard_stop_power(
+ PositionToPower(shooter_.absolute_position()));
if (output) {
- output->latch_piston = latch_piston_;
- output->brake_piston = brake_piston_;
+ output_struct.latch_piston = latch_piston_;
+ output_struct.brake_piston = brake_piston_;
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
if (position) {
- AOS_LOG_STRUCT(
- DEBUG, "internal state",
- ::y2014::control_loops::ShooterStateToLog(
- shooter_.absolute_position(), shooter_.absolute_velocity(), state_,
- position->latch, position->pusher_proximal.current,
- position->pusher_distal.current, position->plunger, brake_piston_,
- latch_piston_));
+ last_position_latch_ = position->latch();
+ last_position_plunger_ = position->plunger();
+ last_position_position_ = position->position();
- last_position_ = *position;
-
- last_distal_posedge_count_ = position->pusher_distal.posedge_count;
- last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
- last_distal_current_ = position->pusher_distal.current;
- last_proximal_current_ = position->pusher_proximal.current;
+ last_distal_posedge_count_ = position->pusher_distal()->posedge_count();
+ last_proximal_posedge_count_ = position->pusher_proximal()->posedge_count();
+ last_distal_current_ = position->pusher_distal()->current();
+ last_proximal_current_ = position->pusher_proximal()->current();
}
- status->absolute_position = shooter_.absolute_position();
- status->absolute_velocity = shooter_.absolute_velocity();
- status->state = state_;
+ status_builder.add_absolute_position(shooter_.absolute_position());
+ status_builder.add_absolute_velocity(shooter_.absolute_velocity());
+ status_builder.add_state(state_);
- status->shots = shot_count_;
+ status_builder.add_shots(shot_count_);
+
+ status->Send(status_builder.Finish());
}
-void ShooterMotor::Zero(::y2014::control_loops::ShooterQueue::Output *output) {
- output->voltage = 0.0;
- output->latch_piston = latch_piston_;
- output->brake_piston = brake_piston_;
+flatbuffers::Offset<Output> ShooterMotor::Zero(
+ aos::Sender<Output>::Builder *output) {
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_voltage(0.0);
+ output_builder.add_latch_piston(latch_piston_);
+ output_builder.add_brake_piston(brake_piston_);
+ return output_builder.Finish();
}
+} // namespace shooter
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index e755b0f..1e7d4aa 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -8,11 +8,15 @@
#include "aos/time/time.h"
#include "y2014/constants.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
#include "y2014/control_loops/shooter/shooter_motor_plant.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/shooter_output_generated.h"
+#include "y2014/control_loops/shooter/shooter_position_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
namespace y2014 {
namespace control_loops {
+namespace shooter {
namespace testing {
class ShooterTest_UnloadWindupPositive_Test;
class ShooterTest_UnloadWindupNegative_Test;
@@ -127,19 +131,17 @@
::std::chrono::milliseconds(40);
class ShooterMotor
- : public aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue> {
+ : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit ShooterMotor(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2014.control_loops.shooter_queue");
+ explicit ShooterMotor(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/shooter");
// True if the goal was moved to avoid goal windup.
bool capped_goal() const { return shooter_.capped_goal(); }
double PowerToPosition(double power);
double PositionToPower(double position);
- void CheckCalibrations(
- const ::y2014::control_loops::ShooterQueue::Position *position);
+ void CheckCalibrations(const Position *position);
typedef enum {
STATE_INITIALIZE = 0,
@@ -159,15 +161,14 @@
State state() { return state_; }
protected:
- void RunIteration(
- const ::y2014::control_loops::ShooterQueue::Goal *goal,
- const ::y2014::control_loops::ShooterQueue::Position *position,
- ::y2014::control_loops::ShooterQueue::Output *output,
- ::y2014::control_loops::ShooterQueue::Status *status) override;
+ void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
// We have to override this to keep the pistons in the correct positions.
- void Zero(::y2014::control_loops::ShooterQueue::Output *output) override;
+ flatbuffers::Offset<Output> Zero(
+ aos::Sender<Output>::Builder *output) override;
// Friend the test classes for acces to the internal state.
friend class testing::ShooterTest_UnloadWindupPositive_Test;
@@ -185,7 +186,9 @@
load_timeout_ = monotonic_now + kLoadTimeout;
}
- ::y2014::control_loops::ShooterQueue::Position last_position_;
+ bool last_position_latch_ = false;
+ bool last_position_plunger_ = false;
+ double last_position_position_ = 0.0;
ZeroedStateFeedbackLoop shooter_;
@@ -232,6 +235,7 @@
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
+} // namespace shooter
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/shooter/shooter.q b/y2014/control_loops/shooter/shooter.q
deleted file mode 100644
index c7ed97d..0000000
--- a/y2014/control_loops/shooter/shooter.q
+++ /dev/null
@@ -1,103 +0,0 @@
-package y2014.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-// Published on ".y2014.control_loops.shooter_queue"
-queue_group ShooterQueue {
- implements aos.control_loops.ControlLoop;
-
- message Output {
- double voltage;
- // true: latch engaged, false: latch open
- bool latch_piston;
- // true: brake engaged false: brake released
- bool brake_piston;
- };
- message Goal {
- // Shot power in joules.
- double shot_power;
- // Shoots as soon as this is true.
- bool shot_requested;
- bool unload_requested;
- bool load_requested;
- };
-
- // Back is when the springs are all the way stretched.
- message Position {
- // In meters, out is positive.
- double position;
-
- // If the latch piston is fired and this hall effect has been triggered, the
- // plunger is all the way back and latched.
- bool plunger;
- // Gets triggered when the pusher is all the way back.
- .frc971.PosedgeOnlyCountedHallEffectStruct pusher_distal;
- // Triggers just before pusher_distal.
- .frc971.PosedgeOnlyCountedHallEffectStruct pusher_proximal;
- // Triggers when the latch engages.
- bool latch;
- };
- message Status {
- // Whether it's ready to shoot right now.
- bool ready;
- // Whether the plunger is in and out of the way of grabbing a ball.
- // TODO(ben): Populate these!
- bool cocked;
- // How many times we've shot.
- int32_t shots;
- bool done;
- // What we think the current position of the hard stop on the shooter is, in
- // shot power (Joules).
- double hard_stop_power;
-
- double absolute_position;
- double absolute_velocity;
- uint32_t state;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
-
-struct ShooterStateToLog {
- double absolute_position;
- double absolute_velocity;
- uint32_t state;
- bool latch_sensor;
- bool proximal;
- bool distal;
- bool plunger;
- bool brake;
- bool latch_piston;
-};
-
-struct ShooterVoltageToLog {
- double X_hat;
- double applied;
-};
-
-struct ShooterMovingGoal {
- double dx;
-};
-
-struct ShooterChangeCalibration {
- double encoder;
- double real_position;
- double old_position;
- double new_position;
- double old_offset;
- double new_offset;
-};
-
-struct ShooterStatusToLog {
- double goal;
- double position;
-};
-
-struct PowerAdjustment {
- double requested_power;
- double actual_power;
-};
diff --git a/y2014/control_loops/shooter/shooter_goal.fbs b/y2014/control_loops/shooter/shooter_goal.fbs
new file mode 100644
index 0000000..3326562
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_goal.fbs
@@ -0,0 +1,12 @@
+namespace y2014.control_loops.shooter;
+
+table Goal {
+ // Shot power in joules.
+ shot_power:double;
+ // Shoots as soon as this is true.
+ shot_requested:bool;
+ unload_requested:bool;
+ load_requested:bool;
+}
+
+root_type Goal;
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index f910b6d..ad24cf9 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -9,7 +9,10 @@
#include "gtest/gtest.h"
#include "y2014/constants.h"
#include "y2014/control_loops/shooter/shooter.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/control_loops/shooter/shooter_output_generated.h"
+#include "y2014/control_loops/shooter/shooter_position_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
#include "y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h"
namespace chrono = ::std::chrono;
@@ -17,6 +20,7 @@
namespace y2014 {
namespace control_loops {
+namespace shooter {
namespace testing {
using ::y2014::control_loops::shooter::kMaxExtension;
@@ -31,11 +35,8 @@
ShooterSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt,
double initial_position)
: event_loop_(event_loop),
- shooter_position_sender_(
- event_loop_->MakeSender<ShooterQueue::Position>(
- ".y2014.control_loops.shooter_queue.position")),
- shooter_output_fetcher_(event_loop_->MakeFetcher<ShooterQueue::Output>(
- ".y2014.control_loops.shooter_queue.output")),
+ shooter_position_sender_(event_loop_->MakeSender<Position>("/shooter")),
+ shooter_output_fetcher_(event_loop_->MakeFetcher<Output>("/shooter")),
shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
latch_piston_state_(false),
latch_delay_count_(0),
@@ -69,6 +70,7 @@
plant->mutable_Y() = plant->C() * plant->X();
last_voltage_ = 0.0;
last_plant_position_ = 0.0;
+ last_position_ = 0.0;
SetPhysicalSensors(&last_position_message_);
}
@@ -89,8 +91,7 @@
// Sets the values of the physical sensors that can be directly observed
// (encoder, hall effect).
- void SetPhysicalSensors(
- ::y2014::control_loops::ShooterQueue::Position *position) {
+ void SetPhysicalSensors(PositionT *position) {
const constants::Values &values = constants::GetValues();
position->position = GetPosition();
@@ -113,17 +114,27 @@
position->plunger =
CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
}
- position->pusher_distal.current =
+
+ if (!position->pusher_distal) {
+ position->pusher_distal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
+ }
+ if (!position->pusher_proximal) {
+ position->pusher_proximal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
+ }
+
+ position->pusher_distal->current =
CheckRange(GetAbsolutePosition(), values.shooter.pusher_distal);
- position->pusher_proximal.current =
+ position->pusher_proximal->current =
CheckRange(GetAbsolutePosition(), values.shooter.pusher_proximal);
}
void UpdateEffectEdge(
- ::frc971::PosedgeOnlyCountedHallEffectStruct *sensor,
- const ::frc971::PosedgeOnlyCountedHallEffectStruct &last_sensor,
+ ::frc971::PosedgeOnlyCountedHallEffectStructT *sensor,
+ const ::frc971::PosedgeOnlyCountedHallEffectStructT &last_sensor,
const constants::Values::AnglePair &limits,
- const ::y2014::control_loops::ShooterQueue::Position &last_position) {
+ float last_position) {
sensor->posedge_count = last_sensor.posedge_count;
sensor->negedge_count = last_sensor.negedge_count;
@@ -131,7 +142,7 @@
if (sensor->current && !last_sensor.current) {
++sensor->posedge_count;
- if (last_position.position + initial_position_ < limits.lower_angle) {
+ if (last_position + initial_position_ < limits.lower_angle) {
AOS_LOG(DEBUG,
"Posedge value on lower edge of sensor, count is now %d\n",
sensor->posedge_count);
@@ -159,8 +170,15 @@
// it into a state using the plunger_in_, brake_in_, and latch_in_ values.
void SendPositionMessage() {
const constants::Values &values = constants::GetValues();
- ::aos::Sender<ShooterQueue::Position>::Message position =
- shooter_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ shooter_position_sender_.MakeBuilder();
+
+ PositionT position;
+
+ position.pusher_distal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
+ position.pusher_proximal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
if (use_passed_) {
plunger_latched_ = latch_in_ && plunger_in_;
@@ -168,43 +186,45 @@
brake_piston_state_ = brake_in_;
}
- SetPhysicalSensors(position.get());
+ SetPhysicalSensors(&position);
- position->latch = latch_piston_state_;
+ position.latch = latch_piston_state_;
// Handle pusher distal hall effect
- UpdateEffectEdge(&position->pusher_distal,
- last_position_message_.pusher_distal,
- values.shooter.pusher_distal, last_position_message_);
+ UpdateEffectEdge(position.pusher_distal.get(),
+ *last_position_message_.pusher_distal.get(),
+ values.shooter.pusher_distal, last_position_);
// Handle pusher proximal hall effect
- UpdateEffectEdge(&position->pusher_proximal,
- last_position_message_.pusher_proximal,
- values.shooter.pusher_proximal, last_position_message_);
+ UpdateEffectEdge(position.pusher_proximal.get(),
+ *last_position_message_.pusher_proximal.get(),
+ values.shooter.pusher_proximal, last_position_);
- last_position_message_ = *position;
- position.Send();
+ builder.Send(Position::Pack(*builder.fbb(), &position));
+
+ last_position_ = position.position;
+ last_position_message_ = std::move(position);
}
// Simulates the claw moving for one timestep.
void Simulate() {
last_plant_position_ = GetAbsolutePosition();
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- if (shooter_output_fetcher_->latch_piston && !latch_piston_state_ &&
+ if (shooter_output_fetcher_->latch_piston() && !latch_piston_state_ &&
latch_delay_count_ <= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = 6;
- } else if (!shooter_output_fetcher_->latch_piston &&
+ } else if (!shooter_output_fetcher_->latch_piston() &&
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = -6;
}
- if (shooter_output_fetcher_->brake_piston && !brake_piston_state_ &&
+ if (shooter_output_fetcher_->brake_piston() && !brake_piston_state_ &&
brake_delay_count_ <= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = 5;
- } else if (!shooter_output_fetcher_->brake_piston &&
+ } else if (!shooter_output_fetcher_->brake_piston() &&
brake_piston_state_ && brake_delay_count_ >= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = -5;
@@ -265,13 +285,13 @@
EXPECT_LE(constants::GetValues().shooter.lower_hard_limit,
GetAbsolutePosition());
- last_voltage_ = shooter_output_fetcher_->voltage;
+ last_voltage_ = shooter_output_fetcher_->voltage();
}
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+ ::aos::Sender<Position> shooter_position_sender_;
+ ::aos::Fetcher<Output> shooter_output_fetcher_;
bool first_ = true;
@@ -300,7 +320,8 @@
double initial_position_;
double last_voltage_;
- ShooterQueue::Position last_position_message_;
+ double last_position_ = 0.0;
+ PositionT last_position_message_;
double last_plant_position_;
};
@@ -310,13 +331,12 @@
protected:
ShooterTestTemplated()
: ::aos::testing::ControlLoopTestTemplated<TestType>(
+ aos::configuration::ReadConfig("y2014/config.json"),
// TODO(austin): I think this runs at 5 ms in real life.
chrono::microseconds(5000)),
test_event_loop_(this->MakeEventLoop()),
- shooter_goal_fetcher_(test_event_loop_->MakeFetcher<ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
- shooter_goal_sender_(test_event_loop_->MakeSender<ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
+ shooter_goal_fetcher_(test_event_loop_->MakeFetcher<Goal>("/shooter")),
+ shooter_goal_sender_(test_event_loop_->MakeSender<Goal>("/shooter")),
shooter_event_loop_(this->MakeEventLoop()),
shooter_motor_(shooter_event_loop_.get()),
shooter_plant_event_loop_(this->MakeEventLoop()),
@@ -330,12 +350,12 @@
void VerifyNearGoal() {
shooter_goal_fetcher_.Fetch();
const double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_goal_fetcher_->shot_power, pos, 1e-4);
+ EXPECT_NEAR(shooter_goal_fetcher_->shot_power(), pos, 1e-4);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<ShooterQueue::Goal> shooter_goal_fetcher_;
- ::aos::Sender<ShooterQueue::Goal> shooter_goal_sender_;
+ ::aos::Fetcher<Goal> shooter_goal_fetcher_;
+ ::aos::Sender<Goal> shooter_goal_sender_;
// Create a loop and simulation plant.
::std::unique_ptr<::aos::EventLoop> shooter_event_loop_;
@@ -381,18 +401,18 @@
TEST_F(ShooterTest, GoesToValue) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(2));
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -400,19 +420,19 @@
TEST_F(ShooterTest, Fire) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1200));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 35.0;
- message->shot_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(35.0);
+ goal_builder.add_shot_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
bool hit_fire = false;
@@ -421,11 +441,12 @@
RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 17.0;
- message->shot_requested = false;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder =
+ shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(17.0);
+ goal_builder.add_shot_requested(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
hit_fire = true;
}
@@ -433,8 +454,9 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
}
@@ -443,20 +465,20 @@
TEST_F(ShooterTest, FireLong) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 0.0;
- message->shot_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(0.0);
+ goal_builder.add_shot_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
bool hit_fire = false;
@@ -465,10 +487,11 @@
RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_requested = false;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder =
+ shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_requested(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
hit_fire = true;
}
@@ -476,8 +499,9 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
}
@@ -487,10 +511,10 @@
TEST_F(ShooterTest, LoadTooFar) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 500.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(500.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(1600))) {
@@ -505,19 +529,19 @@
TEST_F(ShooterTest, MoveGoal) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 14.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(14.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(500));
@@ -525,8 +549,8 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -534,18 +558,18 @@
TEST_F(ShooterTest, Unload) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
@@ -563,10 +587,10 @@
TEST_F(ShooterTest, RezeroWhileUnloading) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
@@ -576,10 +600,10 @@
RunFor(chrono::milliseconds(500));
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
@@ -597,18 +621,18 @@
TEST_F(ShooterTest, UnloadWindupNegative) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
int kicked_delay = 20;
@@ -640,18 +664,18 @@
TEST_F(ShooterTest, UnloadWindupPositive) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
int kicked_delay = 20;
@@ -688,18 +712,18 @@
SetEnabled(true);
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(2));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -710,18 +734,18 @@
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(3));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -741,10 +765,10 @@
bool initialized = false;
Reinitialize(start_pos);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 120.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(120.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::seconds(2))) {
@@ -759,8 +783,8 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -784,5 +808,6 @@
// TODO(austin): Test all the timeouts...
} // namespace testing
+} // namespace shooter
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/shooter/shooter_main.cc b/y2014/control_loops/shooter/shooter_main.cc
index 2da76c3..4161fdf 100644
--- a/y2014/control_loops/shooter/shooter_main.cc
+++ b/y2014/control_loops/shooter/shooter_main.cc
@@ -1,13 +1,16 @@
#include "y2014/control_loops/shooter/shooter.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
- ::y2014::control_loops::ShooterMotor shooter(&event_loop);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2014::control_loops::shooter::ShooterMotor shooter(&event_loop);
event_loop.Run();
diff --git a/y2014/control_loops/shooter/shooter_output.fbs b/y2014/control_loops/shooter/shooter_output.fbs
new file mode 100644
index 0000000..e1900c4
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_output.fbs
@@ -0,0 +1,11 @@
+namespace y2014.control_loops.shooter;
+
+table Output {
+ voltage:double;
+ // true: latch engaged, false: latch open
+ latch_piston:bool;
+ // true: brake engaged false: brake released
+ brake_piston:bool;
+}
+
+root_type Output;
diff --git a/y2014/control_loops/shooter/shooter_position.fbs b/y2014/control_loops/shooter/shooter_position.fbs
new file mode 100644
index 0000000..9c73a1a
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_position.fbs
@@ -0,0 +1,21 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2014.control_loops.shooter;
+
+// Back is when the springs are all the way stretched.
+table Position {
+ // In meters, out is positive.
+ position:double;
+
+ // If the latch piston is fired and this hall effect has been triggered, the
+ // plunger is all the way back and latched.
+ plunger:bool;
+ // Gets triggered when the pusher is all the way back.
+ pusher_distal:frc971.PosedgeOnlyCountedHallEffectStruct;
+ // Triggers just before pusher_distal.
+ pusher_proximal:frc971.PosedgeOnlyCountedHallEffectStruct;
+ // Triggers when the latch engages.
+ latch:bool;
+}
+
+root_type Position;
diff --git a/y2014/control_loops/shooter/shooter_status.fbs b/y2014/control_loops/shooter/shooter_status.fbs
new file mode 100644
index 0000000..4e76e27
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_status.fbs
@@ -0,0 +1,21 @@
+namespace y2014.control_loops.shooter;
+
+table Status {
+ // Whether it's ready to shoot right now.
+ ready:bool;
+ // Whether the plunger is in and out of the way of grabbing a ball.
+ // TODO(ben): Populate these!
+ //cocked:bool;
+ // How many times we've shot.
+ shots:int;
+ //done:bool;
+ // What we think the current position of the hard stop on the shooter is, in
+ // shot power (Joules).
+ hard_stop_power:double;
+
+ absolute_position:double;
+ absolute_velocity:double;
+ state:uint;
+}
+
+root_type Status;
diff --git a/y2014/hot_goal_reader.cc b/y2014/hot_goal_reader.cc
index 773397d..8eb4efb 100644
--- a/y2014/hot_goal_reader.cc
+++ b/y2014/hot_goal_reader.cc
@@ -7,20 +7,22 @@
#include <unistd.h>
#include "aos/byteorder.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/time/time.h"
-#include "y2014/queues/hot_goal.q.h"
+#include "y2014/queues/hot_goal_generated.h"
int main() {
::aos::InitNRT();
- ::aos::ShmEventLoop shm_event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop shm_event_loop(&config.message());
::aos::Sender<::y2014::HotGoal> hot_goal_sender =
- shm_event_loop.MakeSender<::y2014::HotGoal>(".y2014.hot_goal");
+ shm_event_loop.MakeSender<::y2014::HotGoal>("/");
uint64_t left_count = 0, right_count = 0;
int my_socket = -1;
@@ -82,11 +84,12 @@
}
if (data & 0x01) ++right_count;
if (data & 0x02) ++left_count;
- auto message = hot_goal_sender.MakeMessage();
- message->left_count = left_count;
- message->right_count = right_count;
- AOS_LOG_STRUCT(DEBUG, "sending", *message);
- message.Send();
+ auto builder = hot_goal_sender.MakeBuilder();
+ y2014::HotGoal::Builder hot_goal_builder =
+ builder.MakeBuilder<y2014::HotGoal>();
+ hot_goal_builder.add_left_count(left_count);
+ hot_goal_builder.add_right_count(right_count);
+ builder.Send(hot_goal_builder.Finish());
} break;
case 0:
AOS_LOG(WARNING, "read on %d timed out\n", connection);
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index 29cb354..6f94bfe 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -11,14 +11,13 @@
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2014/actors/shoot_actor.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
#include "y2014/control_loops/drivetrain/drivetrain_base.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -158,18 +157,18 @@
event_loop, control_loops::GetDrivetrainConfig(),
::aos::input::DrivetrainInputReader::InputType::kSteeringWheel, {}),
claw_status_fetcher_(
- event_loop->MakeFetcher<::y2014::control_loops::ClawQueue::Status>(
- ".y2014.control_loops.claw_queue.status")),
+ event_loop->MakeFetcher<::y2014::control_loops::claw::Status>(
+ "/claw")),
claw_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::claw::Goal>(
+ "/claw")),
shooter_goal_sender_(
- event_loop->MakeSender<::y2014::control_loops::ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
+ event_loop->MakeSender<::y2014::control_loops::shooter::Goal>(
+ "/shooter")),
drivetrain_status_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
shot_power_(80.0),
goal_angle_(0.0),
separation_angle_(kGrabSeparation),
@@ -326,7 +325,8 @@
}
if (data.PosEdge(kFire)) {
- EnqueueAction(shoot_action_factory_.Make(0.0));
+ aos::common::actions::DoubleParamT param;
+ EnqueueAction(shoot_action_factory_.Make(param));
} else if (data.NegEdge(kFire)) {
CancelCurrentAction();
}
@@ -354,7 +354,7 @@
double goal_angle = goal_angle_;
if (drivetrain_status_fetcher_.get()) {
goal_angle +=
- SpeedToAngleOffset(drivetrain_status_fetcher_->robot_speed);
+ SpeedToAngleOffset(drivetrain_status_fetcher_->robot_speed());
} else {
AOS_LOG_INTERVAL(no_drivetrain_status_);
}
@@ -362,7 +362,7 @@
if (moving_for_shot_) {
claw_status_fetcher_.Fetch();
if (claw_status_fetcher_.get()) {
- if (::std::abs(claw_status_fetcher_->bottom - goal_angle) < 0.2) {
+ if (::std::abs(claw_status_fetcher_->bottom() - goal_angle) < 0.2) {
moving_for_shot_ = false;
separation_angle_ = shot_separation_angle_;
}
@@ -381,26 +381,30 @@
data.IsPressed(kRollersIn) || data.IsPressed(kIntakePosition) ||
data.IsPressed(kIntakeOpenPosition) || data.IsPressed(kCatch);
{
- auto goal_message = claw_goal_sender_.MakeMessage();
- goal_message->bottom_angle = goal_angle;
- goal_message->separation_angle = separation_angle;
- goal_message->intake =
+ auto builder = claw_goal_sender_.MakeBuilder();
+ control_loops::claw::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::claw::Goal>();
+ goal_builder.add_bottom_angle(goal_angle);
+ goal_builder.add_separation_angle(separation_angle);
+ goal_builder.add_intake(
intaking ? 12.0
- : (data.IsPressed(kRollersOut) ? -12.0 : intake_power_);
- goal_message->centering = intaking ? 12.0 : 0.0;
+ : (data.IsPressed(kRollersOut) ? -12.0 : intake_power_));
+ goal_builder.add_centering(intaking ? 12.0 : 0.0);
- if (!goal_message.Send()) {
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending claw goal failed\n");
}
}
{
- auto goal_message = shooter_goal_sender_.MakeMessage();
- goal_message->shot_power = shot_power_;
- goal_message->shot_requested = data.IsPressed(kFire);
- goal_message->unload_requested = data.IsPressed(kUnload);
- goal_message->load_requested = data.IsPressed(kReload);
- if (!goal_message.Send()) {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ control_loops::shooter::Goal::Builder goal_builder =
+ builder.MakeBuilder<control_loops::shooter::Goal>();
+ goal_builder.add_shot_power(shot_power_);
+ goal_builder.add_shot_requested(data.IsPressed(kFire));
+ goal_builder.add_unload_requested(data.IsPressed(kUnload));
+ goal_builder.add_load_requested(data.IsPressed(kReload));
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending shooter goal failed\n");
}
}
@@ -415,11 +419,10 @@
}
private:
- ::aos::Fetcher<::y2014::control_loops::ClawQueue::Status>
- claw_status_fetcher_;
- ::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
- ::aos::Sender<::y2014::control_loops::ShooterQueue::Goal> shooter_goal_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::y2014::control_loops::claw::Status> claw_status_fetcher_;
+ ::aos::Sender<::y2014::control_loops::claw::Goal> claw_goal_sender_;
+ ::aos::Sender<::y2014::control_loops::shooter::Goal> shooter_goal_sender_;
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
double shot_power_;
@@ -443,7 +446,10 @@
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2014::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2014/queues/BUILD b/y2014/queues/BUILD
index 182ab7d..dba212d 100644
--- a/y2014/queues/BUILD
+++ b/y2014/queues/BUILD
@@ -1,24 +1,19 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = 'profile_params',
- srcs = [
- 'profile_params.q',
- ],
+flatbuffer_cc_library(
+ name = "auto_mode_fbs",
+ srcs = [
+ "auto_mode.fbs",
+ ],
+ gen_reflections = 1,
)
-queue_library(
- name = 'hot_goal',
- srcs = [
- 'hot_goal.q',
- ],
-)
-
-queue_library(
- name = 'auto_mode',
- srcs = [
- 'auto_mode.q',
- ],
+flatbuffer_cc_library(
+ name = "hot_goal_fbs",
+ srcs = [
+ "hot_goal.fbs",
+ ],
+ gen_reflections = 1,
)
diff --git a/y2014/queues/auto_mode.fbs b/y2014/queues/auto_mode.fbs
new file mode 100644
index 0000000..bca34f8
--- /dev/null
+++ b/y2014/queues/auto_mode.fbs
@@ -0,0 +1,9 @@
+namespace y2014.sensors;
+
+// Published on "/aos"
+table AutoMode {
+ // Voltage of the analog auto selector knob.
+ voltage:double;
+}
+
+root_type AutoMode;
diff --git a/y2014/queues/auto_mode.q b/y2014/queues/auto_mode.q
deleted file mode 100644
index b07f526..0000000
--- a/y2014/queues/auto_mode.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package y2014.sensors;
-
-// Published on ".y2014.sensors.auto_mode"
-message AutoMode {
- // Voltage of the analog auto selector knob.
- double voltage;
-};
diff --git a/y2014/queues/hot_goal.fbs b/y2014/queues/hot_goal.fbs
new file mode 100644
index 0000000..5c4bd0d
--- /dev/null
+++ b/y2014/queues/hot_goal.fbs
@@ -0,0 +1,9 @@
+namespace y2014;
+
+// Published on "/"
+table HotGoal {
+ left_count:ulong;
+ right_count:ulong;
+}
+
+root_type HotGoal;
diff --git a/y2014/queues/hot_goal.q b/y2014/queues/hot_goal.q
deleted file mode 100644
index 8aabb46..0000000
--- a/y2014/queues/hot_goal.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package y2014;
-
-// Published on ".y2014.hot_goal"
-message HotGoal {
- uint64_t left_count;
- uint64_t right_count;
-};
diff --git a/y2014/queues/profile_params.q b/y2014/queues/profile_params.q
deleted file mode 100644
index 206ceff..0000000
--- a/y2014/queues/profile_params.q
+++ /dev/null
@@ -1,6 +0,0 @@
-package y2014;
-
-struct ProfileParams {
- double velocity;
- double acceleration;
-};
diff --git a/y2014/wpilib_interface.cc b/y2014/wpilib_interface.cc
index 7952adc..48d9c4b 100644
--- a/y2014/wpilib_interface.cc
+++ b/y2014/wpilib_interface.cc
@@ -18,18 +18,18 @@
#include "frc971/wpilib/wpilib_robot_base.h"
#undef ERROR
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/shifter_hall_effect.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -40,21 +40,19 @@
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "y2014/constants.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
-#include "y2014/queues/auto_mode.q.h"
+#include "y2014/control_loops/claw/claw_output_generated.h"
+#include "y2014/control_loops/claw/claw_position_generated.h"
+#include "y2014/control_loops/shooter/shooter_output_generated.h"
+#include "y2014/control_loops/shooter/shooter_position_generated.h"
+#include "y2014/queues/auto_mode_generated.h"
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-using ::y2014::control_loops::ClawQueue;
-using ::y2014::control_loops::ShooterQueue;
+namespace claw = ::y2014::control_loops::claw;
+namespace shooter = ::y2014::control_loops::shooter;
using aos::make_unique;
namespace y2014 {
@@ -118,16 +116,15 @@
public:
SensorReader(::aos::EventLoop *event_loop)
: ::frc971::wpilib::SensorReader(event_loop),
- auto_mode_sender_(event_loop->MakeSender<::y2014::sensors::AutoMode>(
- ".y2014.sensors.auto_mode")),
- shooter_position_sender_(event_loop->MakeSender<ShooterQueue::Position>(
- ".y2014.control_loops.shooter_queue.position")),
- claw_position_sender_(event_loop->MakeSender<ClawQueue::Position>(
- ".y2014.control_loops.claw_queue.position")),
+ auto_mode_sender_(
+ event_loop->MakeSender<::y2014::sensors::AutoMode>("/aos")),
+ shooter_position_sender_(
+ event_loop->MakeSender<shooter::Position>("/shooter")),
+ claw_position_sender_(event_loop->MakeSender<claw::Position>("/claw")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateMediumEncoderFilterHz(kMaximumEncoderPulsesPerSecond);
@@ -249,59 +246,87 @@
const auto &values = constants::GetValues();
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->right_encoder =
- drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->left_encoder =
- -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
- drivetrain_message->right_speed =
- drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder position_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ position_builder.add_right_encoder(
+ drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ position_builder.add_left_encoder(
+ -drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ position_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+ position_builder.add_right_speed(drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
- drivetrain_message->low_left_hall = low_left_drive_hall_->GetVoltage();
- drivetrain_message->high_left_hall = high_left_drive_hall_->GetVoltage();
- drivetrain_message->left_shifter_position =
- hall_translate(values.left_drive, drivetrain_message->low_left_hall,
- drivetrain_message->high_left_hall);
+ const double low_left_hall = low_left_drive_hall_->GetVoltage();
+ const double high_left_hall = high_left_drive_hall_->GetVoltage();
+ position_builder.add_low_left_hall(low_left_hall);
+ position_builder.add_high_left_hall(high_left_hall);
+ position_builder.add_left_shifter_position(
+ hall_translate(values.left_drive, low_left_hall, high_left_hall));
- drivetrain_message->low_right_hall = low_right_drive_hall_->GetVoltage();
- drivetrain_message->high_right_hall =
- high_right_drive_hall_->GetVoltage();
- drivetrain_message->right_shifter_position =
- hall_translate(values.right_drive, drivetrain_message->low_right_hall,
- drivetrain_message->high_right_hall);
+ const double low_right_hall = low_right_drive_hall_->GetVoltage();
+ const double high_right_hall = high_right_drive_hall_->GetVoltage();
+ position_builder.add_low_right_hall(low_right_hall);
+ position_builder.add_high_right_hall(high_right_hall);
+ position_builder.add_right_shifter_position(
+ hall_translate(values.right_drive, low_right_hall, high_right_hall));
- drivetrain_message.Send();
+ builder.Send(position_builder.Finish());
}
{
- auto auto_mode_message = auto_mode_sender_.MakeMessage();
- auto_mode_message->voltage = auto_selector_analog_->GetVoltage();
- auto_mode_message.Send();
+ auto builder = auto_mode_sender_.MakeBuilder();
+ y2014::sensors::AutoMode::Builder auto_builder =
+ builder.MakeBuilder<y2014::sensors::AutoMode>();
+ auto_builder.add_voltage(auto_selector_analog_->GetVoltage());
+ builder.Send(auto_builder.Finish());
}
}
void RunDmaIteration() {
{
- auto shooter_message = shooter_position_sender_.MakeMessage();
- shooter_message->position = shooter_translate(shooter_encoder_->GetRaw());
- shooter_message->plunger = !shooter_plunger_reader_->value();
- shooter_message->latch = !shooter_latch_reader_->value();
+ auto builder = shooter_position_sender_.MakeBuilder();
+ ::frc971::PosedgeOnlyCountedHallEffectStructT pusher_proximal;
CopyShooterPosedgeCounts(shooter_proximal_counter_.get(),
- &shooter_message->pusher_proximal);
- CopyShooterPosedgeCounts(shooter_distal_counter_.get(),
- &shooter_message->pusher_distal);
+ &pusher_proximal);
+ flatbuffers::Offset<::frc971::PosedgeOnlyCountedHallEffectStruct>
+ pusher_proximal_offset =
+ ::frc971::PosedgeOnlyCountedHallEffectStruct::Pack(
+ *builder.fbb(), &pusher_proximal);
- shooter_message.Send();
+ ::frc971::PosedgeOnlyCountedHallEffectStructT pusher_distal;
+ CopyShooterPosedgeCounts(shooter_distal_counter_.get(), &pusher_distal);
+ flatbuffers::Offset<::frc971::PosedgeOnlyCountedHallEffectStruct>
+ pusher_distal_offset =
+ ::frc971::PosedgeOnlyCountedHallEffectStruct::Pack(
+ *builder.fbb(), &pusher_distal);
+
+ control_loops::shooter::Position::Builder position_builder =
+ builder.MakeBuilder<control_loops::shooter::Position>();
+ position_builder.add_position(
+ shooter_translate(shooter_encoder_->GetRaw()));
+ position_builder.add_plunger(!shooter_plunger_reader_->value());
+ position_builder.add_latch(!shooter_latch_reader_->value());
+ position_builder.add_pusher_distal(pusher_distal_offset);
+ position_builder.add_pusher_proximal(pusher_proximal_offset);
+
+ builder.Send(position_builder.Finish());
}
{
- auto claw_message = claw_position_sender_.MakeMessage();
- top_reader_.RunIteration(&claw_message->top);
- bottom_reader_.RunIteration(&claw_message->bottom);
+ auto builder = claw_position_sender_.MakeBuilder();
+ flatbuffers::Offset<control_loops::claw::HalfClawPosition> top_offset =
+ top_reader_.ReadPosition(builder.fbb());
+ flatbuffers::Offset<control_loops::claw::HalfClawPosition> bottom_offset =
+ bottom_reader_.ReadPosition(builder.fbb());
- claw_message.Send();
+ control_loops::claw::Position::Builder position_builder =
+ builder.MakeBuilder<control_loops::claw::Position>();
+
+ position_builder.add_top(top_offset);
+ position_builder.add_bottom(bottom_offset);
+ builder.Send(position_builder.Finish());
}
}
@@ -347,22 +372,42 @@
void Quit() { synchronizer_.Quit(); }
- void RunIteration(control_loops::HalfClawPosition *half_claw_position) {
+ flatbuffers::Offset<control_loops::claw::HalfClawPosition> ReadPosition(
+ flatbuffers::FlatBufferBuilder *fbb) {
const double multiplier = reversed_ ? -1.0 : 1.0;
synchronizer_.RunIteration();
- CopyPosition(front_counter_.get(), &half_claw_position->front);
- CopyPosition(calibration_counter_.get(),
- &half_claw_position->calibration);
- CopyPosition(back_counter_.get(), &half_claw_position->back);
- half_claw_position->position =
- multiplier * claw_translate(synchronized_encoder_->get());
+
+ ::frc971::HallEffectStructT front;
+ CopyPosition(front_counter_.get(), &front);
+ flatbuffers::Offset<::frc971::HallEffectStruct> front_offset =
+ ::frc971::HallEffectStruct::Pack(*fbb, &front);
+
+ ::frc971::HallEffectStructT calibration;
+ CopyPosition(calibration_counter_.get(), &calibration);
+ flatbuffers::Offset<::frc971::HallEffectStruct> calibration_offset =
+ ::frc971::HallEffectStruct::Pack(*fbb, &calibration);
+
+ ::frc971::HallEffectStructT back;
+ CopyPosition(back_counter_.get(), &back);
+ flatbuffers::Offset<::frc971::HallEffectStruct> back_offset =
+ ::frc971::HallEffectStruct::Pack(*fbb, &back);
+
+ control_loops::claw::HalfClawPosition::Builder half_claw_position_builder(
+ *fbb);
+
+ half_claw_position_builder.add_front(front_offset);
+ half_claw_position_builder.add_calibration(calibration_offset);
+ half_claw_position_builder.add_back(back_offset);
+ half_claw_position_builder.add_position(
+ multiplier * claw_translate(synchronized_encoder_->get()));
+ return half_claw_position_builder.Finish();
}
private:
void CopyPosition(const ::frc971::wpilib::EdgeCounter *counter,
- ::frc971::HallEffectStruct *out) {
+ ::frc971::HallEffectStructT *out) {
const double multiplier = reversed_ ? -1.0 : 1.0;
out->current = !counter->polled_value();
@@ -392,7 +437,7 @@
void CopyShooterPosedgeCounts(
const ::frc971::wpilib::DMAEdgeCounter *counter,
- ::frc971::PosedgeOnlyCountedHallEffectStruct *output) {
+ ::frc971::PosedgeOnlyCountedHallEffectStructT *output) {
output->current = !counter->polled_value();
// These are inverted because the hall effects give logical false when
// there's a magnet in front of them.
@@ -403,9 +448,9 @@
}
::aos::Sender<::y2014::sensors::AutoMode> auto_mode_sender_;
- ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
- ::aos::Sender<ClawQueue::Position> claw_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<shooter::Position> shooter_position_sender_;
+ ::aos::Sender<claw::Position> claw_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
::std::unique_ptr<::frc::AnalogInput> auto_selector_analog_;
@@ -433,12 +478,13 @@
SolenoidWriter(::aos::EventLoop *event_loop,
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
- shooter_(event_loop->MakeFetcher<ShooterQueue::Output>(
- ".y2014.control_loops.shooter_queue.output")),
+ shooter_(event_loop->MakeFetcher<shooter::Output>("/shooter")),
drivetrain_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")) {
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")) {
event_loop->set_name("Solenoids");
event_loop->SetRuntimeRealtimePriority(27);
@@ -484,26 +530,27 @@
{
shooter_.Fetch();
if (shooter_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *shooter_);
- shooter_latch_->Set(!shooter_->latch_piston);
- shooter_brake_->Set(!shooter_->brake_piston);
+ shooter_latch_->Set(!shooter_->latch_piston());
+ shooter_brake_->Set(!shooter_->brake_piston());
}
}
{
drivetrain_.Fetch();
if (drivetrain_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- drivetrain_left_->Set(!drivetrain_->left_high);
- drivetrain_right_->Set(!drivetrain_->right_high);
+ drivetrain_left_->Set(!drivetrain_->left_high());
+ drivetrain_right_->Set(!drivetrain_->right_high());
}
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
{
const bool compressor_on = !pressure_switch_->Get();
- to_log.compressor_on = compressor_on;
+ to_log_builder.add_compressor_on(compressor_on);
if (compressor_on) {
compressor_relay_->Set(::frc::Relay::kForward);
} else {
@@ -512,8 +559,8 @@
}
pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ to_log_builder.add_read_solenoids(pcm_->GetAll());
+ builder.Send(to_log_builder.Finish());
}
}
@@ -528,15 +575,17 @@
::std::unique_ptr<::frc::DigitalInput> pressure_switch_;
::std::unique_ptr<::frc::Relay> compressor_relay_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
+ ::aos::Fetcher<shooter::Output> shooter_;
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output> drivetrain_;
+
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
};
class ShooterWriter
- : public ::frc971::wpilib::LoopOutputHandler<ShooterQueue::Output> {
+ : public ::frc971::wpilib::LoopOutputHandler<shooter::Output> {
public:
ShooterWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<ShooterQueue::Output>(
+ : ::frc971::wpilib::LoopOutputHandler<shooter::Output>(
event_loop, ".y2014.control_loops.shooter_queue.output") {}
void set_shooter_talon(::std::unique_ptr<::frc::Talon> t) {
@@ -544,9 +593,8 @@
}
private:
- virtual void Write(const ShooterQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- shooter_talon_->SetSpeed(output.voltage / 12.0);
+ virtual void Write(const shooter::Output &output) override {
+ shooter_talon_->SetSpeed(output.voltage() / 12.0);
}
virtual void Stop() override {
@@ -557,11 +605,10 @@
::std::unique_ptr<::frc::Talon> shooter_talon_;
};
-class ClawWriter
- : public ::frc971::wpilib::LoopOutputHandler<ClawQueue::Output> {
+class ClawWriter : public ::frc971::wpilib::LoopOutputHandler<claw::Output> {
public:
ClawWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<ClawQueue::Output>(
+ : ::frc971::wpilib::LoopOutputHandler<claw::Output>(
event_loop, ".y2014.control_loops.claw_queue.output") {}
void set_top_claw_talon(::std::unique_ptr<::frc::Talon> t) {
@@ -589,14 +636,13 @@
}
private:
- virtual void Write(const ClawQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- intake1_talon_->SetSpeed(output.intake_voltage / 12.0);
- intake2_talon_->SetSpeed(output.intake_voltage / 12.0);
- bottom_claw_talon_->SetSpeed(-output.bottom_claw_voltage / 12.0);
- top_claw_talon_->SetSpeed(output.top_claw_voltage / 12.0);
- left_tusk_talon_->SetSpeed(output.tusk_voltage / 12.0);
- right_tusk_talon_->SetSpeed(-output.tusk_voltage / 12.0);
+ virtual void Write(const claw::Output &output) override {
+ intake1_talon_->SetSpeed(output.intake_voltage() / 12.0);
+ intake2_talon_->SetSpeed(output.intake_voltage() / 12.0);
+ bottom_claw_talon_->SetSpeed(-output.bottom_claw_voltage() / 12.0);
+ top_claw_talon_->SetSpeed(output.top_claw_voltage() / 12.0);
+ left_tusk_talon_->SetSpeed(output.tusk_voltage() / 12.0);
+ right_tusk_talon_->SetSpeed(-output.tusk_voltage() / 12.0);
}
virtual void Stop() override {
@@ -625,19 +671,22 @@
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
// Create this first to make sure it ends up in one of the lower-numbered
@@ -680,12 +729,12 @@
AddLoop(&sensor_reader_event_loop);
// Thread 4.
- ::aos::ShmEventLoop gyro_event_loop;
+ ::aos::ShmEventLoop gyro_event_loop(&config.message());
::frc971::wpilib::GyroSender gyro_sender(&gyro_event_loop);
AddLoop(&gyro_event_loop);
// Thread 5.
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(make_unique<::frc::Talon>(5), true);
drivetrain_writer.set_right_controller0(make_unique<::frc::Talon>(2),
@@ -705,7 +754,7 @@
AddLoop(&output_event_loop);
// Thread 6.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm);
diff --git a/y2014/y2014.json b/y2014/y2014.json
new file mode 100644
index 0000000..38440a0
--- /dev/null
+++ b/y2014/y2014.json
@@ -0,0 +1,54 @@
+{
+ "channels":
+ [
+ {
+ "name": "/shooter",
+ "type": "y2014.control_loops.shooter.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2014.control_loops.shooter.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2014.control_loops.shooter.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2014.control_loops.shooter.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/claw",
+ "type": "y2014.control_loops.claw.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/claw",
+ "type": "y2014.control_loops.claw.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/claw",
+ "type": "y2014.control_loops.claw.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/claw",
+ "type": "y2014.control_loops.claw.Position",
+ "frequency": 200
+ }
+ ],
+ "applications": [
+ {
+ "name": "drivetrain"
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ ]
+}