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/y2016/control_loops/drivetrain/BUILD b/y2016/control_loops/drivetrain/BUILD
index 629c957..e706f25 100644
--- a/y2016/control_loops/drivetrain/BUILD
+++ b/y2016/control_loops/drivetrain/BUILD
@@ -1,5 +1,3 @@
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index 58ba7ce..b4fd9d8 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/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 "y2016/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, ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2016/control_loops/shooter/BUILD b/y2016/control_loops/shooter/BUILD
index 8938a2b..794c4fc 100644
--- a/y2016/control_loops/shooter/BUILD
+++ b/y2016/control_loops/shooter/BUILD
@@ -1,16 +1,37 @@
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,
+)
+
+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(
@@ -52,8 +73,11 @@
"shooter.h",
],
deps = [
+ ":shooter_goal_fbs",
+ ":shooter_output_fbs",
":shooter_plants",
- ":shooter_queue",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop",
],
)
@@ -63,10 +87,13 @@
srcs = [
"shooter_lib_test.cc",
],
+ data = ["//y2016:config.json"],
deps = [
+ ":shooter_goal_fbs",
":shooter_lib",
- ":shooter_queue",
- "//aos:queues",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
@@ -80,9 +107,12 @@
"shooter_main.cc",
],
deps = [
+ ":shooter_goal_fbs",
":shooter_lib",
- ":shooter_queue",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index bf527ec..5156a61 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -2,10 +2,7 @@
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-
#include "y2016/control_loops/shooter/shooter_plant.h"
namespace y2016 {
@@ -49,80 +46,99 @@
loop_->Update(disabled);
}
-void ShooterSide::SetStatus(ShooterSideStatus *status) {
+flatbuffers::Offset<ShooterSideStatus> ShooterSide::SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ ShooterSideStatus::Builder shooter_side_status_builder(*fbb);
// Compute the oldest point in the history.
const int oldest_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
// Compute the distance moved over that time period.
- status->avg_angular_velocity =
+ const double avg_angular_velocity =
(history_[oldest_history_position] - history_[history_position_]) /
(::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
static_cast<double>(kHistoryLength - 1));
+ shooter_side_status_builder.add_avg_angular_velocity(avg_angular_velocity);
- status->angular_velocity = loop_->X_hat(1, 0);
+ shooter_side_status_builder.add_angular_velocity(loop_->X_hat(1, 0));
// Ready if average angular velocity is close to the goal.
- status->ready = (std::abs(loop_->next_R(1, 0) -
- status->avg_angular_velocity) < kTolerance &&
- loop_->next_R(1, 0) > 1.0);
+ shooter_side_status_builder.add_ready(
+ (std::abs(loop_->next_R(1, 0) - avg_angular_velocity) < kTolerance &&
+ loop_->next_R(1, 0) > 1.0));
+
+ return shooter_side_status_builder.Finish();
}
Shooter::Shooter(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<ShooterQueue>(event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
shots_(0),
last_pre_shot_timeout_(::aos::monotonic_clock::min_time) {}
-void Shooter::RunIteration(const ShooterQueue::Goal *goal,
- const ShooterQueue::Position *position,
- ShooterQueue::Output *output,
- ShooterQueue::Status *status) {
+void Shooter::RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const ::aos::monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
if (goal) {
// Update position/goal for our two shooter sides.
- left_.set_goal(goal->angular_velocity);
- right_.set_goal(goal->angular_velocity);
-
- // Turn the lights on if we are supposed to spin.
- if (output) {
- if (::std::abs(goal->angular_velocity) > 0.0) {
- output->lights_on = true;
- if (goal->shooting_forwards) {
- output->forwards_flashlight = true;
- output->backwards_flashlight = false;
- } else {
- output->forwards_flashlight = false;
- output->backwards_flashlight = true;
- }
- }
- if (goal->force_lights_on) {
- output->lights_on = true;
- }
- }
+ left_.set_goal(goal->angular_velocity());
+ right_.set_goal(goal->angular_velocity());
}
- left_.set_position(position->theta_left);
- right_.set_position(position->theta_right);
+ left_.set_position(position->theta_left());
+ right_.set_position(position->theta_right());
left_.Update(output == nullptr);
right_.Update(output == nullptr);
- left_.SetStatus(&status->left);
- right_.SetStatus(&status->right);
- status->ready = (status->left.ready && status->right.ready);
+ flatbuffers::Offset<ShooterSideStatus> left_status_offset =
+ left_.SetStatus(status->fbb());
+ flatbuffers::Offset<ShooterSideStatus> right_status_offset =
+ right_.SetStatus(status->fbb());
+
+ ShooterSideStatus *left_status =
+ GetMutableTemporaryPointer(*status->fbb(), left_status_offset);
+ ShooterSideStatus *right_status =
+ GetMutableTemporaryPointer(*status->fbb(), right_status_offset);
+
+ const bool ready = (left_status->ready() && right_status->ready());
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+ status_builder.add_ready((left_status->ready() && right_status->ready()));
+ status_builder.add_left(left_status_offset);
+ status_builder.add_right(right_status_offset);
if (output) {
- output->voltage_left = left_.voltage();
- output->voltage_right = right_.voltage();
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_voltage_left(left_.voltage());
+ output_builder.add_voltage_right(right_.voltage());
+ // Turn the lights on if we are supposed to spin.
if (goal) {
+ bool lights_on = false;
+ if (::std::abs(goal->angular_velocity()) > 0.0) {
+ lights_on = true;
+ if (goal->shooting_forwards()) {
+ output_builder.add_forwards_flashlight(true);
+ output_builder.add_backwards_flashlight(false);
+ } else {
+ output_builder.add_forwards_flashlight(false);
+ output_builder.add_backwards_flashlight(true);
+ }
+ }
+ if (goal->force_lights_on()) {
+ lights_on = true;
+ }
+ output_builder.add_lights_on(lights_on);
+
bool shoot = false;
switch (state_) {
case ShooterLatchState::PASS_THROUGH:
- if (goal->push_to_shooter) {
- if (::std::abs(goal->angular_velocity) > 10) {
- if (status->ready) {
+ if (goal->push_to_shooter()) {
+ if (::std::abs(goal->angular_velocity()) > 10) {
+ if (ready) {
state_ = ShooterLatchState::WAITING_FOR_SPINDOWN;
shoot = true;
}
@@ -134,22 +150,22 @@
break;
case ShooterLatchState::WAITING_FOR_SPINDOWN:
shoot = true;
- if (left_.velocity() < goal->angular_velocity * 0.9 ||
- right_.velocity() < goal->angular_velocity * 0.9) {
+ if (left_.velocity() < goal->angular_velocity() * 0.9 ||
+ right_.velocity() < goal->angular_velocity() * 0.9) {
state_ = ShooterLatchState::WAITING_FOR_SPINUP;
}
- if (::std::abs(goal->angular_velocity) < 10 ||
+ if (::std::abs(goal->angular_velocity()) < 10 ||
last_pre_shot_timeout_ < monotonic_now) {
state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
break;
case ShooterLatchState::WAITING_FOR_SPINUP:
shoot = true;
- if (left_.velocity() > goal->angular_velocity * 0.95 &&
- right_.velocity() > goal->angular_velocity * 0.95) {
+ if (left_.velocity() > goal->angular_velocity() * 0.95 &&
+ right_.velocity() > goal->angular_velocity() * 0.95) {
state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
- if (::std::abs(goal->angular_velocity) < 10 ||
+ if (::std::abs(goal->angular_velocity()) < 10 ||
last_pre_shot_timeout_ < monotonic_now) {
state_ = ShooterLatchState::INCREMENT_SHOT_COUNT;
}
@@ -160,18 +176,22 @@
break;
case ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE:
shoot = true;
- if (!goal->push_to_shooter) {
+ if (!goal->push_to_shooter()) {
state_ = ShooterLatchState::PASS_THROUGH;
}
break;
}
- output->clamp_open = goal->clamp_open;
- output->push_to_shooter = shoot;
+ output_builder.add_clamp_open(goal->clamp_open());
+ output_builder.add_push_to_shooter(shoot);
}
+
+ output->Send(output_builder.Finish());
}
- status->shots = shots_;
+ status_builder.add_shots(shots_);
+
+ status->Send(status_builder.Finish());
}
} // namespace shooter
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index 9e6968f..07fee84 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -4,11 +4,14 @@
#include <memory>
#include "aos/controls/control_loop.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
#include "y2016/control_loops/shooter/shooter_integral_plant.h"
+#include "y2016/control_loops/shooter/shooter_output_generated.h"
+#include "y2016/control_loops/shooter/shooter_position_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
namespace y2016 {
namespace control_loops {
@@ -28,7 +31,8 @@
void set_position(double current_position);
// Populates the status structure.
- void SetStatus(ShooterSideStatus *status);
+ flatbuffers::Offset<ShooterSideStatus> SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
// Returns the control loop calculated voltage.
double voltage() const;
@@ -53,11 +57,11 @@
DISALLOW_COPY_AND_ASSIGN(ShooterSide);
};
-class Shooter : public ::aos::controls::ControlLoop<ShooterQueue> {
+class Shooter
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Shooter(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2016.control_loops.shooter.shooter_queue");
+ explicit Shooter(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/shooter");
enum class ShooterLatchState {
// Any shoot commands will be passed through without modification.
@@ -73,10 +77,9 @@
};
protected:
- void RunIteration(const ShooterQueue::Goal *goal,
- const ShooterQueue::Position *position,
- ShooterQueue::Output *output,
- ShooterQueue::Status *status) override;
+ void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
ShooterSide left_, right_;
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
deleted file mode 100644
index 583b762..0000000
--- a/y2016/control_loops/shooter/shooter.q
+++ /dev/null
@@ -1,81 +0,0 @@
-package y2016.control_loops.shooter;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct ShooterSideStatus {
- // True if the shooter side is up to speed and stable.
- bool ready;
- // The current average velocity in radians/second.
- double avg_angular_velocity;
- // The current instantaneous filtered velocity in radians/second.
- double angular_velocity;
-};
-
-// Published on ".y2016.control_loops.shooter.shooter_queue"
-queue_group ShooterQueue {
- implements aos.control_loops.ControlLoop;
-
- // All angles are in radians, and angular velocities are in radians/second.
- // For all angular velocities, positive is shooting the ball out of the robot.
-
- message Goal {
- // Angular velocity goals in radians/second.
- double angular_velocity;
-
- bool clamp_open; // True to release our clamp on the ball.
- // True to push the ball into the shooter.
- // If we are in the act of shooting with a goal velocity != 0, wait until it
- // is up to speed, push the ball into the shooter, and then wait until it
- // spins up and down before letting the piston be released.
- bool push_to_shooter;
-
- // Forces the lights on.
- bool force_lights_on;
-
- // If true, the robot is shooting forwards.
- bool shooting_forwards;
- };
-
- message Position {
- // Wheel angle in radians/second.
- double theta_left;
- double theta_right;
- };
-
- message Status {
- // Left side status.
- ShooterSideStatus left;
- // Right side status.
- ShooterSideStatus right;
-
- // True if the shooter is ready. It is better to compare the velocities
- // directly so there isn't confusion on if the goal is up to date.
- bool ready;
-
- // The number of shots that have been fired since the start of the shooter
- // control loop.
- uint32_t shots;
- };
-
- message Output {
- // Voltage in volts of the left and right shooter motors.
- double voltage_left;
- double voltage_right;
-
- // See comments on the identical fields in Goal for details.
- bool clamp_open;
- bool push_to_shooter;
-
- // If true, the lights are on.
- bool lights_on;
-
- bool forwards_flashlight;
- bool backwards_flashlight;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2016/control_loops/shooter/shooter_goal.fbs b/y2016/control_loops/shooter/shooter_goal.fbs
new file mode 100644
index 0000000..f041503
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_goal.fbs
@@ -0,0 +1,23 @@
+namespace y2016.control_loops.shooter;
+
+// All angles are in radians, and angular velocities are in radians/second.
+// For all angular velocities, positive is shooting the ball out of the robot.
+table Goal {
+ // Angular velocity goals in radians/second.
+ angular_velocity:double;
+
+ clamp_open:bool; // True to release our clamp on the ball.
+ // True to push the ball into the shooter.
+ // If we are in the act of shooting with a goal velocity != 0, wait until it
+ // is up to speed, push the ball into the shooter, and then wait until it
+ // spins up and down before letting the piston be released.
+ push_to_shooter:bool;
+
+ // Forces the lights on.
+ force_lights_on:bool;
+
+ // If true, the robot is shooting forwards.
+ shooting_forwards:bool;
+}
+
+root_type Goal;
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
index 30b5166..b432049 100644
--- a/y2016/control_loops/shooter/shooter_lib_test.cc
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -5,13 +5,15 @@
#include <chrono>
#include <memory>
-#include "gtest/gtest.h"
-#include "aos/queue.h"
#include "aos/controls/control_loop_test.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
+#include "gtest/gtest.h"
#include "y2016/control_loops/shooter/shooter.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
+#include "y2016/control_loops/shooter/shooter_output_generated.h"
#include "y2016/control_loops/shooter/shooter_plant.h"
+#include "y2016/control_loops/shooter/shooter_position_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
using ::frc971::control_loops::testing::kTeamNumber;
@@ -50,11 +52,8 @@
// Constructs a shooter simulation.
ShooterSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
: event_loop_(event_loop),
- shooter_position_sender_(
- event_loop_->MakeSender<ShooterQueue::Position>(
- ".y2016.control_loops.shooter.shooter_queue.position")),
- shooter_output_fetcher_(event_loop_->MakeFetcher<ShooterQueue::Output>(
- ".y2016.control_loops.shooter.shooter_queue.output")),
+ shooter_position_sender_(event_loop_->MakeSender<Position>("/shooter")),
+ shooter_output_fetcher_(event_loop_->MakeFetcher<Output>("/shooter")),
shooter_plant_left_(new ShooterPlant(
::y2016::control_loops::shooter::MakeShooterPlant())),
shooter_plant_right_(new ShooterPlant(
@@ -73,12 +72,14 @@
// Sends a queue message with the position of the shooter.
void SendPositionMessage() {
- ::aos::Sender<ShooterQueue::Position>::Message position =
- shooter_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ shooter_position_sender_.MakeBuilder();
- position->theta_left = shooter_plant_left_->Y(0, 0);
- position->theta_right = shooter_plant_right_->Y(0, 0);
- position.Send();
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_theta_left(shooter_plant_left_->Y(0, 0));
+ position_builder.add_theta_right(shooter_plant_right_->Y(0, 0));
+ builder.Send(position_builder.Finish());
}
void set_left_voltage_offset(double voltage_offset) {
@@ -95,9 +96,9 @@
::Eigen::Matrix<double, 1, 1> U_left;
::Eigen::Matrix<double, 1, 1> U_right;
- U_left(0, 0) = shooter_output_fetcher_->voltage_left +
+ U_left(0, 0) = shooter_output_fetcher_->voltage_left() +
shooter_plant_left_->voltage_offset();
- U_right(0, 0) = shooter_output_fetcher_->voltage_right +
+ U_right(0, 0) = shooter_output_fetcher_->voltage_right() +
shooter_plant_right_->voltage_offset();
shooter_plant_left_->Update(U_left);
@@ -107,8 +108,8 @@
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_;
::std::unique_ptr<ShooterPlant> shooter_plant_left_, shooter_plant_right_;
@@ -118,18 +119,16 @@
class ShooterTest : public ::aos::testing::ControlLoopTest {
protected:
ShooterTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2016/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
- shooter_goal_fetcher_(test_event_loop_->MakeFetcher<ShooterQueue::Goal>(
- ".y2016.control_loops.shooter.shooter_queue.goal")),
- shooter_goal_sender_(test_event_loop_->MakeSender<ShooterQueue::Goal>(
- ".y2016.control_loops.shooter.shooter_queue.goal")),
+ shooter_goal_fetcher_(test_event_loop_->MakeFetcher<Goal>("/shooter")),
+ shooter_goal_sender_(test_event_loop_->MakeSender<Goal>("/shooter")),
shooter_status_fetcher_(
- test_event_loop_->MakeFetcher<ShooterQueue::Status>(
- ".y2016.control_loops.shooter.shooter_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/shooter")),
shooter_output_fetcher_(
- test_event_loop_->MakeFetcher<ShooterQueue::Output>(
- ".y2016.control_loops.shooter.shooter_queue.output")),
+ test_event_loop_->MakeFetcher<Output>("/shooter")),
shooter_event_loop_(MakeEventLoop()),
shooter_(shooter_event_loop_.get()),
shooter_plant_event_loop_(MakeEventLoop()),
@@ -144,22 +143,22 @@
EXPECT_TRUE(shooter_goal_fetcher_.get() != nullptr);
EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->left.angular_velocity, 10.0);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->right.angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->left()->angular_velocity(), 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->right()->angular_velocity(), 10.0);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->left.avg_angular_velocity, 10.0);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->right.avg_angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->left()->avg_angular_velocity(), 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->right()->avg_angular_velocity(), 10.0);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<ShooterQueue::Goal> shooter_goal_fetcher_;
- ::aos::Sender<ShooterQueue::Goal> shooter_goal_sender_;
- ::aos::Fetcher<ShooterQueue::Status> shooter_status_fetcher_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+ ::aos::Fetcher<Goal> shooter_goal_fetcher_;
+ ::aos::Sender<Goal> shooter_goal_sender_;
+ ::aos::Fetcher<Status> shooter_status_fetcher_;
+ ::aos::Fetcher<Output> shooter_output_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> shooter_event_loop_;
@@ -173,9 +172,10 @@
TEST_F(ShooterTest, DoesNothing) {
SetEnabled(true);
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 0.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(0.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
@@ -183,8 +183,8 @@
VerifyNearGoal();
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(shooter_output_fetcher_->voltage_left, 0.0);
- EXPECT_EQ(shooter_output_fetcher_->voltage_right, 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_left(), 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_right(), 0.0);
}
// Tests that the shooter spins up to speed and that it then spins down
@@ -193,32 +193,34 @@
SetEnabled(true);
// Spin up.
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 450.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(450.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(1));
VerifyNearGoal();
shooter_status_fetcher_.Fetch();
- EXPECT_TRUE(shooter_status_fetcher_->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->ready());
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 0.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(0.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
// Make sure we don't apply voltage on spin-down.
RunFor(dt());
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left);
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right);
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left());
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right());
// Continue to stop.
RunFor(chrono::seconds(5));
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left);
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right);
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left());
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right());
}
// Tests that the shooter doesn't say it is ready if one side isn't up to speed.
@@ -228,9 +230,10 @@
SetEnabled(true);
// Spin up.
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 20.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(20.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
// Cause problems by adding a voltage error on one side.
shooter_plant_.set_right_voltage_offset(-4.0);
@@ -243,9 +246,9 @@
EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
// Left should be up to speed, right shouldn't.
- EXPECT_TRUE(shooter_status_fetcher_->left.ready);
- EXPECT_FALSE(shooter_status_fetcher_->right.ready);
- EXPECT_FALSE(shooter_status_fetcher_->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->left()->ready());
+ EXPECT_FALSE(shooter_status_fetcher_->right()->ready());
+ EXPECT_FALSE(shooter_status_fetcher_->ready());
RunFor(chrono::seconds(5));
@@ -256,22 +259,23 @@
EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
// Both should be up to speed.
- EXPECT_TRUE(shooter_status_fetcher_->left.ready);
- EXPECT_TRUE(shooter_status_fetcher_->right.ready);
- EXPECT_TRUE(shooter_status_fetcher_->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->left()->ready());
+ EXPECT_TRUE(shooter_status_fetcher_->right()->ready());
+ EXPECT_TRUE(shooter_status_fetcher_->ready());
}
// Tests that the shooter can spin up nicely after being disabled for a while.
TEST_F(ShooterTest, Disabled) {
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 200.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(200.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(shooter_output_fetcher_->voltage_left, 0.0);
- EXPECT_EQ(shooter_output_fetcher_->voltage_right, 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_left(), 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_right(), 0.0);
SetEnabled(true);
RunFor(chrono::seconds(5));
diff --git a/y2016/control_loops/shooter/shooter_main.cc b/y2016/control_loops/shooter/shooter_main.cc
index b9737fd..6e46114 100644
--- a/y2016/control_loops/shooter/shooter_main.cc
+++ b/y2016/control_loops/shooter/shooter_main.cc
@@ -1,12 +1,15 @@
#include "y2016/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;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::control_loops::shooter::Shooter shooter(&event_loop);
event_loop.Run();
diff --git a/y2016/control_loops/shooter/shooter_output.fbs b/y2016/control_loops/shooter/shooter_output.fbs
new file mode 100644
index 0000000..6d7fcdf
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_output.fbs
@@ -0,0 +1,19 @@
+namespace y2016.control_loops.shooter;
+
+table Output {
+ // Voltage in volts of the left and right shooter motors.
+ voltage_left:double;
+ voltage_right:double;
+
+ // See comments on the identical fields in Goal for details.
+ clamp_open:bool;
+ push_to_shooter:bool;
+
+ // If true, the lights are on.
+ lights_on:bool;
+
+ forwards_flashlight:bool;
+ backwards_flashlight:bool;
+}
+
+root_type Output;
diff --git a/y2016/control_loops/shooter/shooter_position.fbs b/y2016/control_loops/shooter/shooter_position.fbs
new file mode 100644
index 0000000..d97268c
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_position.fbs
@@ -0,0 +1,11 @@
+namespace y2016.control_loops.shooter;
+
+// All angles are in radians, and angular velocities are in radians/second.
+// For all angular velocities, positive is shooting the ball out of the robot.
+table Position {
+ // Wheel angle in radians/second.
+ theta_left:double;
+ theta_right:double;
+}
+
+root_type Position;
diff --git a/y2016/control_loops/shooter/shooter_status.fbs b/y2016/control_loops/shooter/shooter_status.fbs
new file mode 100644
index 0000000..6f6262b
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_status.fbs
@@ -0,0 +1,27 @@
+namespace y2016.control_loops.shooter;
+
+table ShooterSideStatus {
+ // True if the shooter side is up to speed and stable.
+ ready:bool;
+ // The current average velocity in radians/second.
+ avg_angular_velocity:double;
+ // The current instantaneous filtered velocity in radians/second.
+ angular_velocity:double;
+}
+
+table Status {
+ // Left side status.
+ left:ShooterSideStatus;
+ // Right side status.
+ right:ShooterSideStatus;
+
+ // True if the shooter is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ ready:bool;
+
+ // The number of shots that have been fired since the start of the shooter
+ // control loop.
+ shots:uint;
+}
+
+root_type Status;
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 6a14a5f..4048cdc 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -1,15 +1,42 @@
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 = "superstructure_queue",
+flatbuffer_cc_library(
+ name = "superstructure_goal_fbs",
srcs = [
- "superstructure.q",
+ "superstructure_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
],
)
@@ -73,8 +100,11 @@
"superstructure_controls.h",
],
deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
":superstructure_plants",
- ":superstructure_queue",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
"//aos/controls:control_loop",
"//aos/util:trapezoid_profile",
@@ -83,7 +113,7 @@
"//frc971/control_loops:state_feedback_loop",
"//frc971/zeroing",
"//y2016:constants",
- "//y2016/queues:ball_detector",
+ "//y2016/queues:ball_detector_fbs",
],
)
@@ -92,11 +122,14 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = ["//y2016:config.json"],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_lib",
- ":superstructure_queue",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
- "//aos:queues",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//aos/time",
@@ -112,8 +145,7 @@
],
deps = [
":superstructure_lib",
- ":superstructure_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 99a130c..8f4d02e 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -1,13 +1,12 @@
#include "y2016/control_loops/superstructure/superstructure.h"
-#include "y2016/control_loops/superstructure/superstructure_controls.h"
#include "aos/commonmath.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "y2016/control_loops/superstructure/integral_intake_plant.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/superstructure/integral_intake_plant.h"
+#include "y2016/control_loops/superstructure/superstructure_controls.h"
+#include "y2016/queues/ball_detector_generated.h"
#include "y2016/constants.h"
@@ -229,11 +228,11 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
collision_avoidance_(&intake_, &arm_) {}
bool Superstructure::IsArmNear(double shoulder_tolerance,
@@ -289,10 +288,10 @@
}
void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) {
+ const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const State state_before_switch = state_;
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
@@ -304,8 +303,8 @@
// Bool to track if we should turn the motors on or not.
bool disable = output == nullptr;
- arm_.Correct(position->shoulder, position->wrist);
- intake_.Correct(position->intake);
+ arm_.Correct(position->shoulder(), position->wrist());
+ intake_.Correct(*position->intake());
// There are 2 main zeroing paths, HIGH_ARM_ZERO and LOW_ARM_ZERO.
//
@@ -571,14 +570,14 @@
4.0, // Shoulder acceleration,
4.0, // Wrist velocity
10.0); // Wrist acceleration.
- intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
- unsafe_goal->max_angular_acceleration_intake);
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(),
+ unsafe_goal->max_angular_acceleration_intake());
requested_shoulder =
- ::std::max(unsafe_goal->angle_shoulder,
+ ::std::max(unsafe_goal->angle_shoulder(),
constants::Values::kShoulderRange.lower);
requested_wrist = 0.0;
- requested_intake = unsafe_goal->angle_intake;
+ requested_intake = unsafe_goal->angle_intake();
// Transition to landing once the profile is close to finished for the
// shoulder.
if (arm_.goal(0, 0) > kShoulderTransitionToLanded + 1e-4 ||
@@ -591,18 +590,18 @@
}
} else {
// Otherwise, give the user what he asked for.
- arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
- unsafe_goal->max_angular_acceleration_shoulder,
- unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_wrist);
- intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
- unsafe_goal->max_angular_acceleration_intake);
+ arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder(),
+ unsafe_goal->max_angular_acceleration_shoulder(),
+ unsafe_goal->max_angular_velocity_wrist(),
+ unsafe_goal->max_angular_acceleration_wrist());
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(),
+ unsafe_goal->max_angular_acceleration_intake());
// Except, don't let the shoulder go all the way down.
- requested_shoulder = ::std::max(unsafe_goal->angle_shoulder,
+ requested_shoulder = ::std::max(unsafe_goal->angle_shoulder(),
kShoulderTransitionToLanded);
- requested_wrist = unsafe_goal->angle_wrist;
- requested_intake = unsafe_goal->angle_intake;
+ requested_wrist = unsafe_goal->angle_wrist();
+ requested_intake = unsafe_goal->angle_intake();
// Transition to landing once the profile is close to finished for the
// shoulder.
@@ -641,9 +640,9 @@
if (unsafe_goal) {
constexpr float kTriggerThreshold = 12.0 * 0.90 / 0.005;
- if (unsafe_goal->voltage_climber > 1.0) {
+ if (unsafe_goal->voltage_climber() > 1.0) {
kill_shoulder_accumulator_ +=
- ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber);
+ ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber());
} else {
kill_shoulder_accumulator_ = 0.0;
}
@@ -673,24 +672,28 @@
}
// Calculate the loops for a cycle.
+ double intake_position_power;
+ double intake_velocity_power;
{
Eigen::Matrix<double, 3, 1> error = intake_.controller().error();
- status->intake.position_power =
+ intake_position_power =
intake_.controller().controller().K(0, 0) * error(0, 0);
- status->intake.velocity_power =
+ intake_velocity_power =
intake_.controller().controller().K(0, 1) * error(1, 0);
}
+ double shoulder_position_power;
+ double shoulder_velocity_power;
+ double wrist_position_power;
+ double wrist_velocity_power;
{
Eigen::Matrix<double, 6, 1> error = arm_.controller().error();
- status->shoulder.position_power =
+ shoulder_position_power =
arm_.controller().controller().K(0, 0) * error(0, 0);
- status->shoulder.velocity_power =
+ shoulder_velocity_power =
arm_.controller().controller().K(0, 1) * error(1, 0);
- status->wrist.position_power =
- arm_.controller().controller().K(0, 2) * error(2, 0);
- status->wrist.velocity_power =
- arm_.controller().controller().K(0, 3) * error(3, 0);
+ wrist_position_power = arm_.controller().controller().K(0, 2) * error(2, 0);
+ wrist_velocity_power = arm_.controller().controller().K(0, 3) * error(3, 0);
}
arm_.Update(disable);
@@ -698,98 +701,135 @@
// Write out all the voltages.
if (output) {
- output->voltage_intake = intake_.voltage();
- output->voltage_shoulder = arm_.shoulder_voltage();
- output->voltage_wrist = arm_.wrist_voltage();
+ OutputT output_struct;
+ output_struct.voltage_intake = intake_.voltage();
+ output_struct.voltage_shoulder = arm_.shoulder_voltage();
+ output_struct.voltage_wrist = arm_.wrist_voltage();
- output->voltage_top_rollers = 0.0;
- output->voltage_bottom_rollers = 0.0;
+ output_struct.voltage_top_rollers = 0.0;
+ output_struct.voltage_bottom_rollers = 0.0;
- output->voltage_climber = 0.0;
- output->unfold_climber = false;
+ output_struct.voltage_climber = 0.0;
+ output_struct.unfold_climber = false;
if (unsafe_goal) {
// Ball detector lights.
ball_detector_fetcher_.Fetch();
bool ball_detected = false;
if (ball_detector_fetcher_.get()) {
- ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ ball_detected = ball_detector_fetcher_->voltage() > 2.5;
}
// Climber.
- output->voltage_climber = ::std::max(
+ output_struct.voltage_climber = ::std::max(
static_cast<float>(0.0),
- ::std::min(unsafe_goal->voltage_climber, kMaxClimberVoltage));
- output->unfold_climber = unsafe_goal->unfold_climber;
+ ::std::min(unsafe_goal->voltage_climber(), kMaxClimberVoltage));
+ output_struct.unfold_climber = unsafe_goal->unfold_climber();
// Intake.
- if (unsafe_goal->force_intake || !ball_detected) {
- output->voltage_top_rollers = ::std::max(
+ if (unsafe_goal->force_intake() || !ball_detected) {
+ output_struct.voltage_top_rollers = ::std::max(
-kMaxIntakeTopVoltage,
- ::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
- output->voltage_bottom_rollers =
+ ::std::min(unsafe_goal->voltage_top_rollers(), kMaxIntakeTopVoltage));
+ output_struct.voltage_bottom_rollers =
::std::max(-kMaxIntakeBottomVoltage,
- ::std::min(unsafe_goal->voltage_bottom_rollers,
+ ::std::min(unsafe_goal->voltage_bottom_rollers(),
kMaxIntakeBottomVoltage));
} else {
- output->voltage_top_rollers = 0.0;
- output->voltage_bottom_rollers = 0.0;
+ output_struct.voltage_top_rollers = 0.0;
+ output_struct.voltage_bottom_rollers = 0.0;
}
// Traverse.
- output->traverse_unlatched = unsafe_goal->traverse_unlatched;
- output->traverse_down = unsafe_goal->traverse_down;
+ output_struct.traverse_unlatched = unsafe_goal->traverse_unlatched();
+ output_struct.traverse_down = unsafe_goal->traverse_down();
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
// Save debug/internal state.
- status->zeroed = arm_.zeroed() && intake_.zeroed();
+ flatbuffers::Offset<frc971::EstimatorState> shoulder_estimator_state_offset =
+ arm_.EstimatorState(status->fbb(), 0);
- status->shoulder.angle = arm_.X_hat(0, 0);
- status->shoulder.angular_velocity = arm_.X_hat(1, 0);
- status->shoulder.goal_angle = arm_.goal(0, 0);
- status->shoulder.goal_angular_velocity = arm_.goal(1, 0);
- status->shoulder.unprofiled_goal_angle = arm_.unprofiled_goal(0, 0);
- status->shoulder.unprofiled_goal_angular_velocity =
- arm_.unprofiled_goal(1, 0);
- status->shoulder.voltage_error = arm_.X_hat(4, 0);
- status->shoulder.calculated_velocity =
- (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
- status->shoulder.estimator_state = arm_.EstimatorState(0);
+ JointState::Builder shoulder_builder = status->MakeBuilder<JointState>();
- status->wrist.angle = arm_.X_hat(2, 0);
- status->wrist.angular_velocity = arm_.X_hat(3, 0);
- status->wrist.goal_angle = arm_.goal(2, 0);
- status->wrist.goal_angular_velocity = arm_.goal(3, 0);
- status->wrist.unprofiled_goal_angle = arm_.unprofiled_goal(2, 0);
- status->wrist.unprofiled_goal_angular_velocity = arm_.unprofiled_goal(3, 0);
- status->wrist.voltage_error = arm_.X_hat(5, 0);
- status->wrist.calculated_velocity =
- (arm_.wrist_angle() - last_wrist_angle_) / 0.005;
- status->wrist.estimator_state = arm_.EstimatorState(1);
+ shoulder_builder.add_angle(arm_.X_hat(0, 0));
+ shoulder_builder.add_angular_velocity(arm_.X_hat(1, 0));
+ shoulder_builder.add_goal_angle(arm_.goal(0, 0));
+ shoulder_builder.add_goal_angular_velocity(arm_.goal(1, 0));
+ shoulder_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(0, 0));
+ shoulder_builder.add_unprofiled_goal_angular_velocity(
+ arm_.unprofiled_goal(1, 0));
+ shoulder_builder.add_voltage_error(arm_.X_hat(4, 0));
+ shoulder_builder.add_calculated_velocity(
+ (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005);
+ shoulder_builder.add_position_power(shoulder_position_power);
+ shoulder_builder.add_velocity_power(shoulder_velocity_power);
+ shoulder_builder.add_estimator_state(shoulder_estimator_state_offset);
- status->intake.angle = intake_.X_hat(0, 0);
- status->intake.angular_velocity = intake_.X_hat(1, 0);
- status->intake.goal_angle = intake_.goal(0, 0);
- status->intake.goal_angular_velocity = intake_.goal(1, 0);
- status->intake.unprofiled_goal_angle = intake_.unprofiled_goal(0, 0);
- status->intake.unprofiled_goal_angular_velocity =
- intake_.unprofiled_goal(1, 0);
- status->intake.calculated_velocity =
- (intake_.position() - last_intake_angle_) / 0.005;
- status->intake.voltage_error = intake_.X_hat(2, 0);
- status->intake.estimator_state = intake_.EstimatorState(0);
- status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
+ flatbuffers::Offset<JointState> shoulder_offset = shoulder_builder.Finish();
- status->shoulder_controller_index = arm_.controller_index();
+ flatbuffers::Offset<frc971::EstimatorState> wrist_estimator_state_offset =
+ arm_.EstimatorState(status->fbb(), 1);
+
+ JointState::Builder wrist_builder = status->MakeBuilder<JointState>();
+
+ wrist_builder.add_angle(arm_.X_hat(2, 0));
+ wrist_builder.add_angular_velocity(arm_.X_hat(3, 0));
+ wrist_builder.add_goal_angle(arm_.goal(2, 0));
+ wrist_builder.add_goal_angular_velocity(arm_.goal(3, 0));
+ wrist_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(2, 0));
+ wrist_builder.add_unprofiled_goal_angular_velocity(
+ arm_.unprofiled_goal(3, 0));
+ wrist_builder.add_voltage_error(arm_.X_hat(5, 0));
+ wrist_builder.add_calculated_velocity(
+ (arm_.wrist_angle() - last_wrist_angle_) / 0.005);
+ wrist_builder.add_position_power(wrist_position_power);
+ wrist_builder.add_velocity_power(wrist_velocity_power);
+ wrist_builder.add_estimator_state(wrist_estimator_state_offset);
+
+ flatbuffers::Offset<JointState> wrist_offset = wrist_builder.Finish();
+
+ flatbuffers::Offset<frc971::EstimatorState> intake_estimator_state_offset =
+ intake_.EstimatorState(status->fbb(), 0);
+
+ JointState::Builder intake_builder = status->MakeBuilder<JointState>();
+ intake_builder.add_position_power(intake_position_power);
+ intake_builder.add_velocity_power(intake_velocity_power);
+ intake_builder.add_angle(intake_.X_hat(0, 0));
+ intake_builder.add_angular_velocity(intake_.X_hat(1, 0));
+ intake_builder.add_goal_angle(intake_.goal(0, 0));
+ intake_builder.add_goal_angular_velocity(intake_.goal(1, 0));
+ intake_builder.add_unprofiled_goal_angle(intake_.unprofiled_goal(0, 0));
+ intake_builder.add_unprofiled_goal_angular_velocity(
+ intake_.unprofiled_goal(1, 0));
+ intake_builder.add_calculated_velocity(
+ (intake_.position() - last_intake_angle_) / 0.005);
+ intake_builder.add_voltage_error(intake_.X_hat(2, 0));
+ intake_builder.add_estimator_state(intake_estimator_state_offset);
+ intake_builder.add_feedforwards_power(intake_.controller().ff_U(0, 0));
+
+ flatbuffers::Offset<JointState> intake_offset = intake_builder.Finish();
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_shoulder(shoulder_offset);
+ status_builder.add_wrist(wrist_offset);
+ status_builder.add_intake(intake_offset);
+
+ status_builder.add_zeroed(arm_.zeroed() && intake_.zeroed());
+ status_builder.add_shoulder_controller_index(arm_.controller_index());
last_shoulder_angle_ = arm_.shoulder_angle();
last_wrist_angle_ = arm_.wrist_angle();
last_intake_angle_ = intake_.position();
- status->estopped = (state_ == ESTOP);
+ status_builder.add_estopped((state_ == ESTOP));
- status->state = state_;
- status->is_collided = is_collided;
+ status_builder.add_state(state_);
+ status_builder.add_is_collided(is_collided);
+
+ status->Send(status_builder.Finish());
last_state_ = state_before_switch;
}
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 610db8e..2219c95 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -8,9 +8,12 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/control_loops/superstructure/superstructure_controls.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
namespace y2016 {
namespace control_loops {
@@ -105,11 +108,10 @@
};
class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2016.control_loops.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
static constexpr double kZeroingVoltage = 6.0;
static constexpr double kShooterHangingVoltage = 6.0;
@@ -209,11 +211,9 @@
bool collided() const { return collision_avoidance_.collided(); }
protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) override;
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
deleted file mode 100644
index d784929..0000000
--- a/y2016/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,145 +0,0 @@
-package y2016.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct JointState {
- // Angle of the joint in radians.
- float angle;
- // Angular velocity of the joint in radians/second.
- float angular_velocity;
- // Profiled goal angle of the joint in radians.
- float goal_angle;
- // Profiled goal angular velocity of the joint in radians/second.
- float goal_angular_velocity;
- // Unprofiled goal angle of the joint in radians.
- float unprofiled_goal_angle;
- // Unprofiled goal angular velocity of the joint in radians/second.
- float unprofiled_goal_angular_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.EstimatorState estimator_state;
-};
-
-// Published on ".y2016.control_loops.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Zero on the intake is when the horizontal tube stock members are level
- // with the top frame rails of the robot. This will be essentially when we
- // are in the intaking position. Positive is up. The angle is measured
- // relative to the top
- // of the robot frame.
- // Zero on the shoulder is when the shoulder is down against the hard stop
- // blocks. Positive is up. The angle is measured relative to the top of
- // the robot frame.
- // Zero on the wrist is horizontal and landed in the bellypan. Positive is
- // the same direction as the shoulder. The angle is measured relative to
- // the top of the robot frame. For calibration, 0 is measured as parallel
- // to the big frame supporting the shooter.
-
- // Goal angles and angular velocities of the superstructure subsystems.
- double angle_intake;
- double angle_shoulder;
- // In relation to the ground plane.
- double angle_wrist;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- float max_angular_velocity_intake;
- float max_angular_velocity_shoulder;
- float max_angular_velocity_wrist;
-
- float max_angular_acceleration_intake;
- float max_angular_acceleration_shoulder;
- float max_angular_acceleration_wrist;
-
- // Voltage to send to the rollers. Positive is sucking in.
- float voltage_top_rollers;
- float voltage_bottom_rollers;
-
- // Voltage to sent to the climber. Positive is pulling the robot up.
- float voltage_climber;
- // If true, unlatch the climber and allow it to unfold.
- bool unfold_climber;
-
- bool force_intake;
-
- // If true, release the latch which holds the traverse mechanism in the
- // middle.
- bool traverse_unlatched;
- // If true, fire the traverse mechanism down.
- bool traverse_down;
- };
-
- message Status {
- // Are the superstructure subsystems zeroed?
- bool zeroed;
-
- // If true, we have aborted.
- bool estopped;
-
- // The internal state of the state machine.
- int32_t state;
-
-
- // Estimated angles and angular velocities of the superstructure subsystems.
- JointState intake;
- JointState shoulder;
- JointState wrist;
-
- int32_t shoulder_controller_index;
-
- // Is the superstructure collided?
- bool is_collided;
- };
-
- message Position {
- // Zero for the intake potentiometer value is horizontal, and positive is
- // up.
- // Zero for the shoulder potentiometer value is horizontal, and positive is
- // up.
- // Zero for the wrist potentiometer value is parallel to the arm and with
- // the shooter wheels pointed towards the shoulder joint. This is measured
- // relative to the arm, not the ground, not like the world we actually
- // present to users.
- .frc971.PotAndIndexPosition intake;
- .frc971.PotAndIndexPosition shoulder;
- .frc971.PotAndIndexPosition wrist;
- };
-
- message Output {
- float voltage_intake;
- float voltage_shoulder;
- float voltage_wrist;
-
- float voltage_top_rollers;
- float voltage_bottom_rollers;
-
- // Voltage to sent to the climber. Positive is pulling the robot up.
- float voltage_climber;
- // If true, release the latch to trigger the climber to unfold.
- bool unfold_climber;
-
- // If true, release the latch to hold the traverse mechanism in the middle.
- bool traverse_unlatched;
- // If true, fire the traverse mechanism down.
- bool traverse_down;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index b8f3a48..269089b 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -1,6 +1,5 @@
#include "y2016/control_loops/superstructure/superstructure_controls.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
#include "y2016/control_loops/superstructure/integral_intake_plant.h"
@@ -96,10 +95,10 @@
// TODO(austin): Handle zeroing errors.
-void Arm::Correct(PotAndIndexPosition position_shoulder,
- PotAndIndexPosition position_wrist) {
- estimators_[kShoulderIndex].UpdateEstimate(position_shoulder);
- estimators_[kWristIndex].UpdateEstimate(position_wrist);
+void Arm::Correct(const PotAndIndexPosition *position_shoulder,
+ const PotAndIndexPosition *position_wrist) {
+ estimators_[kShoulderIndex].UpdateEstimate(*position_shoulder);
+ estimators_[kWristIndex].UpdateEstimate(*position_wrist);
// Handle zeroing errors
if (estimators_[kShoulderIndex].error()) {
@@ -130,7 +129,7 @@
}
{
- Y_ << position_shoulder.encoder, position_wrist.encoder;
+ Y_ << position_shoulder->encoder(), position_wrist->encoder();
Y_ += offset_;
loop_->Correct(Y_);
}
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 8936650..d301054 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -11,7 +11,7 @@
#include "frc971/zeroing/zeroing.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
namespace y2016 {
namespace control_loops {
@@ -111,8 +111,8 @@
Arm();
// Updates our estimator with the latest position.
- void Correct(::frc971::PotAndIndexPosition position_shoulder,
- ::frc971::PotAndIndexPosition position_wrist);
+ void Correct(const ::frc971::PotAndIndexPosition *position_shoulder,
+ const ::frc971::PotAndIndexPosition *position_wrist);
// Forces the goal to be the provided goal.
void ForceGoal(double unprofiled_goal_shoulder, double unprofiled_goal_wrist);
diff --git a/y2016/control_loops/superstructure/superstructure_goal.fbs b/y2016/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..4274bd8
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,50 @@
+namespace y2016.control_loops.superstructure;
+
+table Goal {
+ // Zero on the intake is when the horizontal tube stock members are level
+ // with the top frame rails of the robot. This will be essentially when we
+ // are in the intaking position. Positive is up. The angle is measured
+ // relative to the top
+ // of the robot frame.
+ // Zero on the shoulder is when the shoulder is down against the hard stop
+ // blocks. Positive is up. The angle is measured relative to the top of
+ // the robot frame.
+ // Zero on the wrist is horizontal and landed in the bellypan. Positive is
+ // the same direction as the shoulder. The angle is measured relative to
+ // the top of the robot frame. For calibration, 0 is measured as parallel
+ // to the big frame supporting the shooter.
+
+ // Goal angles and angular velocities of the superstructure subsystems.
+ angle_intake:double;
+ angle_shoulder:double;
+ // In relation to the ground plane.
+ angle_wrist:double;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ max_angular_velocity_intake:float;
+ max_angular_velocity_shoulder:float;
+ max_angular_velocity_wrist:float;
+
+ max_angular_acceleration_intake:float;
+ max_angular_acceleration_shoulder:float;
+ max_angular_acceleration_wrist:float;
+
+ // Voltage to send to the rollers. Positive is sucking in.
+ voltage_top_rollers:float;
+ voltage_bottom_rollers:float;
+
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ voltage_climber:float;
+ // If true, unlatch the climber and allow it to unfold.
+ unfold_climber:bool;
+
+ force_intake:bool;
+
+ // If true, release the latch which holds the traverse mechanism in the
+ // middle.
+ traverse_unlatched:bool;
+ // If true, fire the traverse mechanism down.
+ traverse_down:bool;
+}
+
+root_type Goal;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 79b4d24..d530cb3 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -7,14 +7,16 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
#include "aos/time/time.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2016/control_loops/superstructure/arm_plant.h"
#include "y2016/control_loops/superstructure/intake_plant.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
#include "y2016/constants.h"
@@ -85,14 +87,11 @@
: event_loop_(event_loop),
dt_(dt),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2016.control_loops.superstructure_queue.position")),
+ event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2016.control_loops.superstructure_queue.output")),
+ event_loop_->MakeFetcher<Output>("/superstructure")),
intake_plant_(new IntakePlant(MakeIntakePlant())),
arm_plant_(new ArmPlant(MakeArmPlant())),
pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
@@ -144,14 +143,32 @@
// Sends a queue message with the position.
void SendPositionMessage() {
- ::aos::Sender<control_loops::SuperstructureQueue::Position>::Message
- position = superstructure_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
- pot_encoder_intake_.GetSensorValues(&position->intake);
- pot_encoder_shoulder_.GetSensorValues(&position->shoulder);
- pot_encoder_wrist_.GetSensorValues(&position->wrist);
+ frc971::PotAndIndexPosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
- position.Send();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> intake_offset =
+ pot_encoder_intake_.GetSensorValues(&intake_builder);
+
+ frc971::PotAndIndexPosition::Builder shoulder_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> shoulder_offset =
+ pot_encoder_shoulder_.GetSensorValues(&shoulder_builder);
+
+ frc971::PotAndIndexPosition::Builder wrist_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> wrist_offset =
+ pot_encoder_wrist_.GetSensorValues(&wrist_builder);
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_intake(intake_offset);
+ position_builder.add_shoulder(shoulder_offset);
+ position_builder.add_wrist(wrist_offset);
+
+ builder.Send(position_builder.Finish());
}
double shoulder_angle() const { return arm_plant_->X(0, 0); }
@@ -180,37 +197,39 @@
// Feed voltages into physics simulation.
::Eigen::Matrix<double, 1, 1> intake_U;
::Eigen::Matrix<double, 2, 1> arm_U;
- intake_U << superstructure_output_fetcher_->voltage_intake +
+ intake_U << superstructure_output_fetcher_->voltage_intake() +
intake_plant_->voltage_offset();
- arm_U << superstructure_output_fetcher_->voltage_shoulder +
+ arm_U << superstructure_output_fetcher_->voltage_shoulder() +
arm_plant_->shoulder_voltage_offset(),
- superstructure_output_fetcher_->voltage_wrist +
+ superstructure_output_fetcher_->voltage_wrist() +
arm_plant_->wrist_voltage_offset();
// Verify that the correct power limits are being respected depending on
// which mode we are in.
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
- if (superstructure_status_fetcher_->state == Superstructure::RUNNING ||
- superstructure_status_fetcher_->state ==
+ if (superstructure_status_fetcher_->state() == Superstructure::RUNNING ||
+ superstructure_status_fetcher_->state() ==
Superstructure::LANDING_RUNNING) {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ AOS_CHECK_LE(
+ ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kOperatingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
Superstructure::kOperatingVoltage);
} else {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ AOS_CHECK_LE(
+ ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kZeroingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
Superstructure::kZeroingVoltage);
}
if (arm_plant_->X(0, 0) <=
Superstructure::kShoulderTransitionToLanded + 1e-4) {
- AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder,
+ AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder(),
Superstructure::kLandingShoulderDownVoltage - 0.00001);
}
@@ -309,9 +328,9 @@
bool first_ = true;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
::std::unique_ptr<IntakePlant> intake_plant_;
::std::unique_ptr<ArmPlant> arm_plant_;
@@ -334,17 +353,16 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2016/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
superstructure_goal_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
superstructure_goal_sender_(
- test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
superstructure_status_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop()),
@@ -357,26 +375,26 @@
EXPECT_TRUE(superstructure_goal_fetcher_.get() != nullptr);
EXPECT_TRUE(superstructure_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
- superstructure_status_fetcher_->intake.angle, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake(),
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder(),
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist(),
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake(),
superstructure_plant_.intake_angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder(),
superstructure_plant_.shoulder_angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist(),
superstructure_plant_.wrist_angle(), 0.001);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -390,17 +408,18 @@
TEST_F(SuperstructureTest, DoesNothing) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0;
- message->angle_shoulder = 0;
- message->angle_wrist = 0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0);
+ goal_builder.add_angle_shoulder(0);
+ goal_builder.add_angle_wrist(0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -412,17 +431,18 @@
SetEnabled(true);
// Set a reasonable goal.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI / 4.0;
- message->angle_shoulder = 1.4;
- message->angle_wrist = M_PI / 4.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI / 4.0);
+ goal_builder.add_angle_shoulder(1.4);
+ goal_builder.add_angle_wrist(M_PI / 4.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -437,43 +457,45 @@
SetEnabled(true);
// Set some ridiculous goals to test upper limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI * 10;
- message->angle_shoulder = M_PI * 10;
- message->angle_wrist = M_PI * 10;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI * 10);
+ goal_builder.add_angle_shoulder(M_PI * 10);
+ goal_builder.add_angle_wrist(M_PI * 10);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kWristRange.upper +
constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
// Set some ridiculous goals to test limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI * 10;
- message->angle_shoulder = M_PI * 10;
- message->angle_wrist = -M_PI * 10.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI * 10);
+ goal_builder.add_angle_shoulder(M_PI * 10);
+ goal_builder.add_angle_wrist(-M_PI * 10.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -481,26 +503,27 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kWristRange.lower +
constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
// Set some ridiculous goals to test lower limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = -M_PI * 10;
- message->angle_shoulder = -M_PI * 10;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(-M_PI * 10);
+ goal_builder.add_angle_shoulder(-M_PI * 10);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -508,27 +531,28 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.lower,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist()->angle(), 0.001);
}
// Tests that the loop zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -557,12 +581,13 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.lower);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = constants::Values::kWristRange.upper +
- constants::Values::kShoulderRange.upper;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.upper);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.upper +
+ constants::Values::kShoulderRange.upper);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// We have to wait for it to put the elevator in a safe position as well.
RunFor(chrono::seconds(15));
@@ -580,11 +605,12 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// We have to wait for it to put the superstructure in a safe position as
// well.
@@ -604,11 +630,12 @@
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower + 0.3;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower + 0.3);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
@@ -628,11 +655,13 @@
superstructure_plant_.set_check_for_collisions(false);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower + 0.03;
- message->angle_shoulder = constants::Values::kShoulderRange.lower + 0.03;
- message->angle_wrist = constants::Values::kWristRange.lower + 0.03;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower + 0.03);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower +
+ 0.03);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.lower + 0.03);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(100));
@@ -659,17 +688,18 @@
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = constants::Values::kWristRange.upper;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.upper);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.upper);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Expected states to cycle through and check in order.
@@ -730,17 +760,18 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = constants::Values::kWristRange.lower;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.lower);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Expected states to cycle through and check in order.
@@ -812,11 +843,12 @@
superstructure_plant_.InitializeRelativeWristPosition(0.0);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
@@ -834,12 +866,13 @@
superstructure_plant_.InitializeRelativeWristPosition(-0.001);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- constants::Values::kShoulderEncoderIndexDifference * 10;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ constants::Values::kShoulderEncoderIndexDifference * 10);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Run disabled for 2 seconds
@@ -883,17 +916,18 @@
TEST_F(SuperstructureTest, ShoulderAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -902,17 +936,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.5;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.5);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// TODO(austin): The profile isn't feasible, so when we try to track it, we
@@ -930,17 +965,18 @@
TEST_F(SuperstructureTest, IntakeAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -949,17 +985,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.5;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.5);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_acceleration(1.20);
@@ -974,18 +1011,19 @@
TEST_F(SuperstructureTest, WristAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -994,18 +1032,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.5;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.5);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_acceleration(1.05);
@@ -1021,18 +1060,19 @@
TEST_F(SuperstructureTest, SaturatedIntakeProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1041,18 +1081,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.5;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 4.5;
- message->max_angular_acceleration_intake = 800;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 100;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 100;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.5);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(4.5);
+ goal_builder.add_max_angular_acceleration_intake(800);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(100);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(100);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(4.65);
@@ -1068,17 +1109,18 @@
TEST_F(SuperstructureTest, SaturatedShoulderProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1087,17 +1129,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.9;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1.0;
- message->max_angular_acceleration_intake = 1.0;
- message->max_angular_velocity_shoulder = 5.0;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 100;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.9);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1.0);
+ goal_builder.add_max_angular_acceleration_intake(1.0);
+ goal_builder.add_max_angular_velocity_shoulder(5.0);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(100);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(1.0);
@@ -1113,18 +1156,19 @@
TEST_F(SuperstructureTest, SaturatedWristProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1133,18 +1177,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 1.3;
- message->max_angular_velocity_intake = 1.0;
- message->max_angular_acceleration_intake = 1.0;
- message->max_angular_velocity_shoulder = 1.0;
- message->max_angular_acceleration_shoulder = 1.0;
- message->max_angular_velocity_wrist = 10.0;
- message->max_angular_acceleration_wrist = 160.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(1.3);
+ goal_builder.add_max_angular_velocity_intake(1.0);
+ goal_builder.add_max_angular_acceleration_intake(1.0);
+ goal_builder.add_max_angular_velocity_shoulder(1.0);
+ goal_builder.add_max_angular_acceleration_shoulder(1.0);
+ goal_builder.add_max_angular_velocity_wrist(10.0);
+ goal_builder.add_max_angular_acceleration_wrist(160.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(1.0);
@@ -1165,21 +1210,26 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = constants::Values::kShoulderRange.lower; // Down
- message->angle_wrist = 0.0; // Stowed
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(
+ constants::Values::kShoulderRange.lower); // Down
+ goal_builder.add_angle_wrist(0.0); // Stowed
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = M_PI / 4.0; // in the collision area
- message->angle_wrist = M_PI / 2.0; // down
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(M_PI / 4.0); // in the collision area
+ goal_builder.add_angle_wrist(M_PI / 2.0); // down
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1188,27 +1238,30 @@
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be out of the way despite being told to move to stowing.
- EXPECT_LT(superstructure_status_fetcher_->intake.angle, M_PI);
- EXPECT_LT(superstructure_status_fetcher_->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(), M_PI);
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(),
constants::Values::kIntakeRange.upper);
- EXPECT_LT(superstructure_status_fetcher_->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(),
CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
// The arm should have reached its goal.
- EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder()->angle(),
+ 0.001);
// The wrist should be forced into a stowing position.
// Since the intake is kicked out, we can be within
// kMaxWristAngleForMovingByIntake
- EXPECT_NEAR(0, superstructure_status_fetcher_->wrist.angle,
+ EXPECT_NEAR(0, superstructure_status_fetcher_->wrist()->angle(),
CollisionAvoidance::kMaxWristAngleForMovingByIntake + 0.001);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = M_PI / 2.0; // in the collision area
- message->angle_wrist = M_PI; // forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(M_PI / 2.0); // in the collision area
+ goal_builder.add_angle_wrist(M_PI); // forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1224,11 +1277,12 @@
superstructure_plant_.InitializeAbsoluteWristPosition(M_PI); // forward
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = M_PI; // intentionally asking for forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(M_PI); // intentionally asking for forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
@@ -1237,12 +1291,12 @@
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be in intaking position, as asked.
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake()->angle(), 0.001);
// The shoulder and wrist should both be at zero degrees (i.e.
// stowed/intaking position).
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist()->angle(), 0.001);
}
// Make sure that we can properly detect a collision.
@@ -1250,11 +1304,12 @@
SetEnabled(true);
// Zero & go straight up with the shoulder.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI * 0.5;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI * 0.5);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1290,11 +1345,12 @@
SetEnabled(true);
// Zero & go straight up with the shoulder.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1333,11 +1389,12 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0; // intentionally asking for forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0); // intentionally asking for forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1353,7 +1410,7 @@
RunFor(chrono::seconds(2));
superstructure_goal_fetcher_.Fetch();
EXPECT_LE(constants::Values::kShoulderRange.lower,
- superstructure_goal_fetcher_->angle_shoulder);
+ superstructure_goal_fetcher_->angle_shoulder());
}
// Make sure that we land slowly.
@@ -1361,27 +1418,29 @@
SetEnabled(true);
// Zero & go to initial position.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI * 0.25;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI * 0.25);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
// Tell it to land in the bellypan as fast as possible.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Wait until we hit the transition point.
@@ -1400,27 +1459,29 @@
SetEnabled(true);
// Zero & go to initial position.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
// Tell it to take off as fast as possible.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI / 2.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI / 2.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Wait until we hit the transition point.
diff --git a/y2016/control_loops/superstructure/superstructure_main.cc b/y2016/control_loops/superstructure/superstructure_main.cc
index 00fa7dd..abe2ec9 100644
--- a/y2016/control_loops/superstructure/superstructure_main.cc
+++ b/y2016/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2016/control_loops/superstructure/superstructure.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;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2016/control_loops/superstructure/superstructure_output.fbs b/y2016/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..40a0091
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,22 @@
+namespace y2016.control_loops.superstructure;
+
+table Output {
+ voltage_intake:float;
+ voltage_shoulder:float;
+ voltage_wrist:float;
+
+ voltage_top_rollers:float;
+ voltage_bottom_rollers:float;
+
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ voltage_climber:float;
+ // If true, release the latch to trigger the climber to unfold.
+ unfold_climber:bool;
+
+ // If true, release the latch to hold the traverse mechanism in the middle.
+ traverse_unlatched:bool;
+ // If true, fire the traverse mechanism down.
+ traverse_down:bool;
+}
+
+root_type Output;
diff --git a/y2016/control_loops/superstructure/superstructure_position.fbs b/y2016/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..fb356e0
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,19 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2016.control_loops.superstructure;
+
+table Position {
+ // Zero for the intake potentiometer value is horizontal, and positive is
+ // up.
+ // Zero for the shoulder potentiometer value is horizontal, and positive is
+ // up.
+ // Zero for the wrist potentiometer value is parallel to the arm and with
+ // the shooter wheels pointed towards the shoulder joint. This is measured
+ // relative to the arm, not the ground, not like the world we actually
+ // present to users.
+ intake:frc971.PotAndIndexPosition;
+ shoulder:frc971.PotAndIndexPosition;
+ wrist:frc971.PotAndIndexPosition;
+}
+
+root_type Position;
diff --git a/y2016/control_loops/superstructure/superstructure_status.fbs b/y2016/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..373bfe2
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,56 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2016.control_loops.superstructure;
+
+table JointState {
+ // Angle of the joint in radians.
+ angle:float;
+ // Angular velocity of the joint in radians/second.
+ angular_velocity:float;
+ // Profiled goal angle of the joint in radians.
+ goal_angle:float;
+ // Profiled goal angular velocity of the joint in radians/second.
+ goal_angular_velocity:float;
+ // Unprofiled goal angle of the joint in radians.
+ unprofiled_goal_angle:float;
+ // Unprofiled goal angular velocity of the joint in radians/second.
+ unprofiled_goal_angular_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.EstimatorState;
+}
+
+table Status {
+ // Are the superstructure subsystems zeroed?
+ zeroed:bool;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // The internal state of the state machine.
+ state:int;
+
+
+ // Estimated angles and angular velocities of the superstructure subsystems.
+ intake:JointState;
+ shoulder:JointState;
+ wrist:JointState;
+
+ shoulder_controller_index:int;
+
+ // Is the superstructure collided?
+ is_collided:bool;
+}
+
+root_type Status;