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/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index f43b448..7cbb202 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -1,16 +1,48 @@
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:profiled_subsystem_queue",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_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",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
],
)
@@ -24,11 +56,14 @@
],
deps = [
":collision_avoidance",
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
":vacuum",
"//aos/controls:control_loop",
- "//aos/events:event-loop",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//aos/events:event_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2019:constants",
"//y2019:status_light",
],
@@ -39,18 +74,23 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = [
+ "//y2019: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",
"//frc971/control_loops:capped_test_plant",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2019:status_light",
"//y2019/control_loops/superstructure/intake:intake_plants",
],
@@ -64,7 +104,7 @@
deps = [
":superstructure_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
@@ -77,9 +117,12 @@
"collision_avoidance.h",
],
deps = [
- ":superstructure_queue",
- "//aos/controls:control_loop_queues",
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
+ "//aos/controls:control_loop",
"//frc971:constants",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -92,8 +135,11 @@
"vacuum.h",
],
deps = [
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
"//aos/controls:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -104,7 +150,8 @@
],
deps = [
":collision_avoidance",
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
"//aos/testing:googletest",
],
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index eb5b591..9c2bf93 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -1,7 +1,11 @@
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include <cmath>
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace control_loops {
@@ -30,9 +34,9 @@
clear_max_intake_goal();
}
-bool CollisionAvoidance::IsCollided(const SuperstructureQueue::Status *status) {
- return IsCollided(status->wrist.position, status->elevator.position,
- status->intake.position, status->has_piece);
+bool CollisionAvoidance::IsCollided(const Status *status) {
+ return IsCollided(status->wrist()->position(), status->elevator()->position(),
+ status->intake()->position(), status->has_piece());
}
bool CollisionAvoidance::IsCollided(const double wrist_position,
@@ -73,13 +77,12 @@
return false;
}
-void CollisionAvoidance::UpdateGoal(
- const SuperstructureQueue::Status *status,
- const SuperstructureQueue::Goal *unsafe_goal) {
- const double wrist_position = status->wrist.position;
- const double elevator_position = status->elevator.position;
- const double intake_position = status->intake.position;
- const bool has_piece = status->has_piece;
+void CollisionAvoidance::UpdateGoal(const Status *status,
+ const Goal *unsafe_goal) {
+ const double wrist_position = status->wrist()->position();
+ const double elevator_position = status->elevator()->position();
+ const double intake_position = status->intake()->position();
+ const bool has_piece = status->has_piece();
// Start with our constraints being wide open.
clear_max_wrist_goal();
@@ -155,8 +158,8 @@
}
if (unsafe_goal) {
- const double wrist_goal = unsafe_goal->wrist.unsafe_goal;
- const double intake_goal = unsafe_goal->intake.unsafe_goal;
+ const double wrist_goal = unsafe_goal->wrist()->unsafe_goal();
+ const double intake_goal = unsafe_goal->intake()->unsafe_goal();
// Compute if we need to move the intake.
const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 7feb45d..338864e 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -2,9 +2,12 @@
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
#include <cmath>
-#include "aos/controls/control_loops.q.h"
+
#include "frc971/constants.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace control_loops {
@@ -18,15 +21,14 @@
CollisionAvoidance();
// Reports if the superstructure is collided.
- bool IsCollided(const SuperstructureQueue::Status *status);
+ bool IsCollided(const Status *status);
bool IsCollided(double wrist_position, double elevator_position,
double intake_position, bool has_piece);
// Checks and alters goals to make sure they're safe.
// TODO(austin): Either we will have a unit delay because this has to happen
// after the controls, or we need to be more clever about restructuring.
- void UpdateGoal(const SuperstructureQueue::Status *status,
- const SuperstructureQueue::Goal *unsafe_goal);
+ void UpdateGoal(const Status *status, const Goal *unsafe_goal);
// Returns the goals to give to the respective control loops in
// superstructure.
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 82bff4c..6b1f031 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -1,14 +1,21 @@
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "aos/commonmath.h"
+#include "aos/flatbuffers.h"
#include "gtest/gtest.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
namespace testing {
+using aos::FlatbufferDetachedBuffer;
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
/*
Test List:
FullClockwiseRotationFromBottomBackIntakeIn
@@ -22,48 +29,142 @@
QuarterCounterClockwiseRotationFromBottomFrontIntakeMoving
*/
+FlatbufferDetachedBuffer<Goal> MakeZeroGoal() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal> wrist_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder wrist_builder(fbb);
+
+ wrist_builder.add_unsafe_goal(0.0);
+ wrist_offset = wrist_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder elevator_builder(fbb);
+
+ elevator_builder.add_unsafe_goal(0.0);
+ elevator_offset = elevator_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_builder(fbb);
+
+ intake_builder.add_unsafe_goal(0.0);
+ intake_offset = intake_builder.Finish();
+ }
+
+ superstructure::Goal::Builder goal_builder(fbb);
+
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_intake(intake_offset);
+
+ fbb.Finish(goal_builder.Finish());
+ return fbb.Release();
+}
+
+FlatbufferDetachedBuffer<Status> MakeZeroStatus() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus> wrist_offset;
+ {
+ PotAndAbsoluteEncoderProfiledJointStatus::Builder wrist_builder(fbb);
+
+ wrist_builder.add_position(0.0);
+ wrist_offset = wrist_builder.Finish();
+ }
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ elevator_offset;
+ {
+ PotAndAbsoluteEncoderProfiledJointStatus::Builder elevator_builder(fbb);
+
+ elevator_builder.add_position(0.0);
+ elevator_offset = elevator_builder.Finish();
+ }
+
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
+ intake_offset;
+ {
+ AbsoluteEncoderProfiledJointStatus::Builder intake_builder(fbb);
+
+ intake_builder.add_position(0.0);
+ intake_offset = intake_builder.Finish();
+ }
+
+ superstructure::Status::Builder goal_builder(fbb);
+
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_has_piece(false);
+
+ fbb.Finish(goal_builder.Finish());
+ return fbb.Release();
+}
+
class CollisionAvoidanceTests : public ::testing::Test,
public ::testing::WithParamInterface<bool> {
public:
- CollisionAvoidanceTests() { status.has_piece = GetParam(); }
+ CollisionAvoidanceTests()
+ : unsafe_goal_(MakeZeroGoal()), status_(MakeZeroStatus()) {
+ status_.mutable_message()->mutate_has_piece(GetParam());
+ }
void Iterate() {
- SuperstructureQueue::Goal safe_goal;
- bool was_collided = avoidance.IsCollided(&status);
+ FlatbufferDetachedBuffer<Goal> safe_goal = MakeZeroGoal();
+ bool was_collided = avoidance.IsCollided(&status_.message());
while (true) {
- avoidance.UpdateGoal(&status, &unsafe_goal);
+ avoidance.UpdateGoal(&status_.message(), &unsafe_goal_.message());
if (!was_collided) {
- EXPECT_FALSE(avoidance.IsCollided(&status));
+ EXPECT_FALSE(avoidance.IsCollided(&status_.message()));
} else {
- was_collided = avoidance.IsCollided(&status);
+ was_collided = avoidance.IsCollided(&status_.message());
}
- safe_goal.wrist.unsafe_goal =
- ::aos::Clip(unsafe_goal.wrist.unsafe_goal, avoidance.min_wrist_goal(),
- avoidance.max_wrist_goal());
+ safe_goal.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ ::aos::Clip(unsafe_goal_.message().wrist()->unsafe_goal(),
+ avoidance.min_wrist_goal(), avoidance.max_wrist_goal()));
- safe_goal.elevator.unsafe_goal = ::std::max(
- unsafe_goal.elevator.unsafe_goal, avoidance.min_elevator_goal());
+ safe_goal.mutable_message()->mutable_elevator()->mutate_unsafe_goal(
+ ::std::max(unsafe_goal_.message().elevator()->unsafe_goal(),
+ avoidance.min_elevator_goal()));
- safe_goal.intake.unsafe_goal =
- ::aos::Clip(unsafe_goal.intake.unsafe_goal,
- avoidance.min_intake_goal(), avoidance.max_intake_goal());
+ safe_goal.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ ::aos::Clip(unsafe_goal_.message().intake()->unsafe_goal(),
+ avoidance.min_intake_goal(),
+ avoidance.max_intake_goal()));
- LimitedMove(&status.wrist.position, safe_goal.wrist.unsafe_goal);
- LimitedMove(&status.elevator.position, safe_goal.elevator.unsafe_goal);
- LimitedMove(&status.intake.position, safe_goal.intake.unsafe_goal);
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ LimitedMove(status_.message().wrist()->position(),
+ safe_goal.message().wrist()->unsafe_goal()));
+ status_.mutable_message()->mutable_elevator()->mutate_position(
+ LimitedMove(status_.message().elevator()->position(),
+ safe_goal.message().elevator()->unsafe_goal()));
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ LimitedMove(status_.message().intake()->position(),
+ safe_goal.message().intake()->unsafe_goal()));
if (IsMoving()) {
break;
}
- past_status = status;
+ past_wrist_position_ = status_.message().wrist()->position();
+ past_elevator_position_ = status_.message().elevator()->position();
+ past_intake_position_ = status_.message().intake()->position();
}
}
bool IsMoving() {
- if ((past_status.wrist.position == status.wrist.position) &&
- (past_status.elevator.position == status.elevator.position) &&
- (past_status.intake.position == status.intake.position)) {
+ if ((past_wrist_position_ == status_.message().wrist()->position()) &&
+ (past_elevator_position_ == status_.message().elevator()->position()) &&
+ (past_intake_position_ == status_.message().intake()->position())) {
return true;
} else {
return false;
@@ -71,9 +172,11 @@
}
// provide goals and status messages
- SuperstructureQueue::Goal unsafe_goal;
- SuperstructureQueue::Status status;
- SuperstructureQueue::Status past_status;
+ FlatbufferDetachedBuffer<Goal> unsafe_goal_;
+ FlatbufferDetachedBuffer<Status> status_;
+ float past_wrist_position_ = 0;
+ float past_elevator_position_ = 0;
+ float past_intake_position_ = 0;
protected:
// setup for all tests
@@ -81,20 +184,22 @@
void CheckGoals() {
// check to see if we reached the goals
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
- ASSERT_NEAR(unsafe_goal.elevator.unsafe_goal, status.elevator.position,
- 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().elevator()->unsafe_goal(),
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
private:
- void LimitedMove(float *position, double goal) {
- if (*position + kIterationMove < goal) {
- *position += kIterationMove;
- } else if (*position - kIterationMove > goal) {
- *position -= kIterationMove;
+ float LimitedMove(float position, double goal) {
+ if (position + kIterationMove < goal) {
+ return position + kIterationMove;
+ } else if (position - kIterationMove > goal) {
+ return position - kIterationMove;
} else {
- *position = goal;
+ return goal;
}
}
@@ -105,17 +210,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the bottom
// with the intake in and the wrist low back
- status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMinAngle + avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
Iterate();
@@ -128,18 +235,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position =
- avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0));
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle);
Iterate();
@@ -151,18 +259,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- status.wrist.position = avoidance.kWristMaxAngle / 2.0;
- status.elevator.position = 0.5;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle / 2.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.5);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
@@ -174,18 +283,20 @@
FullCounterClockwiseRotationFromBottomBackIntakeIn) {
// sets the status position messgaes to be have the elevator at the bottom
// with the intake in and the wrist low back
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMinAngle + avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMinAngle + avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
Iterate();
@@ -198,17 +309,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0));
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
@@ -221,18 +334,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0));
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.5;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.5);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
Iterate();
@@ -244,23 +358,27 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal = 4.0;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(4.0);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.45;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.45);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Unreasonable Wrist Goal
@@ -268,89 +386,101 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- status.elevator.position = 0.45;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMinAngle + avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.45);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
Iterate();
- EXPECT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ EXPECT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
EXPECT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- EXPECT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ EXPECT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Fix Collision Wrist in Elevator
TEST_P(CollisionAvoidanceTests, FixElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal = 3.14;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(3.14);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes
- status.wrist.position = 4.0;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(4.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Fix Collision Wrist in Intake
TEST_P(CollisionAvoidanceTests, FixWristCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
// sets the status position messgaes
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Fix Collision Wrist Above Elevator
TEST_P(CollisionAvoidanceTests, FixWristElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal = 0.0;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal ( 0.0);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal ( 0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal (
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes
- status.wrist.position = 0.0;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position ( 0.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position ( 0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position (
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
INSTANTIATE_TEST_CASE_P(CollisionAvoidancePieceTest, CollisionAvoidanceTests,
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index ff9bb13..91e1704 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -1,34 +1,36 @@
#include "y2019/control_loops/superstructure/superstructure.h"
-#include "aos/controls/control_loops.q.h"
-#include "aos/events/event-loop.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
-#include "y2019/status_light.q.h"
+#include "y2019/status_light_generated.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
status_light_sender_(
- event_loop->MakeSender<::y2019::StatusLight>(".y2019.status_light")),
+ event_loop->MakeSender<::y2019::StatusLight>("/superstructure")),
drivetrain_status_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
elevator_(constants::GetValues().elevator.subsystem_params),
wrist_(constants::GetValues().wrist.subsystem_params),
intake_(constants::GetValues().intake),
stilts_(constants::GetValues().stilts.subsystem_params) {}
-void Superstructure::RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- SuperstructureQueue::Status *status) {
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
elevator_.Reset();
@@ -37,49 +39,92 @@
stilts_.Reset();
}
- elevator_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->elevator) : nullptr,
- &(position->elevator),
- output != nullptr ? &(output->elevator_voltage) : nullptr,
- &(status->elevator));
+ OutputT output_struct;
- wrist_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->wrist) : nullptr,
- &(position->wrist),
- output != nullptr ? &(output->wrist_voltage) : nullptr,
- &(status->wrist));
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ elevator_status_offset = elevator_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->elevator() : nullptr,
+ position->elevator(),
+ output != nullptr ? &(output_struct.elevator_voltage) : nullptr,
+ status->fbb());
- intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake_joint),
- output != nullptr ? &(output->intake_joint_voltage) : nullptr,
- &(status->intake));
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ wrist_status_offset = wrist_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->wrist() : nullptr,
+ position->wrist(),
+ output != nullptr ? &(output_struct.wrist_voltage) : nullptr,
+ status->fbb());
- stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
- &(position->stilts),
- output != nullptr ? &(output->stilts_voltage) : nullptr,
- &(status->stilts));
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
+ intake_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
+ position->intake_joint(),
+ output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
+ status->fbb());
- vacuum_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
- position->suction_pressure, output, &(status->has_piece),
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ stilts_status_offset = stilts_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->stilts() : nullptr,
+ position->stilts(),
+ output != nullptr ? &(output_struct.stilts_voltage) : nullptr,
+ status->fbb());
+
+ bool has_piece;
+ vacuum_.Iterate(unsafe_goal != nullptr ? unsafe_goal->suction() : nullptr,
+ position->suction_pressure(), &output_struct, &has_piece,
event_loop());
- status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
- status->intake.zeroed && status->stilts.zeroed;
+ bool zeroed;
+ bool estopped;
+ {
+ PotAndAbsoluteEncoderProfiledJointStatus *elevator_status =
+ GetMutableTemporaryPointer(*status->fbb(), elevator_status_offset);
+ PotAndAbsoluteEncoderProfiledJointStatus *wrist_status =
+ GetMutableTemporaryPointer(*status->fbb(), wrist_status_offset);
+ AbsoluteEncoderProfiledJointStatus *intake_status =
+ GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
+ PotAndAbsoluteEncoderProfiledJointStatus *stilts_status =
+ GetMutableTemporaryPointer(*status->fbb(), stilts_status_offset);
- status->estopped = status->elevator.estopped || status->wrist.estopped ||
- status->intake.estopped || status->stilts.estopped;
+ zeroed = elevator_status->zeroed() && wrist_status->zeroed() &&
+ intake_status->zeroed() && stilts_status->zeroed();
+
+ estopped = elevator_status->estopped() || wrist_status->estopped() ||
+ intake_status->estopped() || stilts_status->estopped();
+ }
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+ status_builder.add_has_piece(has_piece);
+
+ status_builder.add_elevator(elevator_status_offset);
+ status_builder.add_wrist(wrist_status_offset);
+ status_builder.add_intake(intake_status_offset);
+ status_builder.add_stilts(stilts_status_offset);
+
+ flatbuffers::Offset<Status> status_offset = status_builder.Finish();
+
+ Status *status_flatbuffer =
+ GetMutableTemporaryPointer(*status->fbb(), status_offset);
if (output) {
- if (unsafe_goal && status->intake.position > kMinIntakeAngleForRollers) {
- output->intake_roller_voltage = unsafe_goal->roller_voltage;
+ if (unsafe_goal &&
+ status_flatbuffer->intake()->position() > kMinIntakeAngleForRollers) {
+ output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
} else {
- output->intake_roller_voltage = 0.0;
+ output_struct.intake_roller_voltage = 0.0;
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
if (unsafe_goal) {
- if (!unsafe_goal->suction.grab_piece) {
+ if (!unsafe_goal->has_suction() || !unsafe_goal->suction()->grab_piece()) {
wrist_.set_controller_index(0);
elevator_.set_controller_index(0);
- } else if (unsafe_goal->suction.gamepiece_mode == 0) {
+ } else if (unsafe_goal->suction()->gamepiece_mode() == 0) {
wrist_.set_controller_index(1);
elevator_.set_controller_index(1);
} else {
@@ -90,7 +135,7 @@
// TODO(theo) move these up when Iterate() is split
// update the goals
- collision_avoidance_.UpdateGoal(status, unsafe_goal);
+ collision_avoidance_.UpdateGoal(status_flatbuffer, unsafe_goal);
elevator_.set_min_position(collision_avoidance_.min_elevator_goal());
wrist_.set_min_position(collision_avoidance_.min_wrist_goal());
@@ -102,23 +147,23 @@
if (status && unsafe_goal) {
// Light Logic
- if (status->estopped) {
+ if (status_flatbuffer->estopped()) {
// Estop is red
SendColors(1.0, 0.0, 0.0);
} else if (drivetrain_status_fetcher_.get() &&
- drivetrain_status_fetcher_->line_follow_logging.frozen) {
+ drivetrain_status_fetcher_->line_follow_logging()->frozen()) {
// Vision align is flashing white for button pressed, purple for target
// acquired.
++line_blink_count_;
if (line_blink_count_ < 20) {
- if (drivetrain_status_fetcher_->line_follow_logging.have_target) {
+ if (drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
SendColors(1.0, 0.0, 1.0);
} else {
SendColors(1.0, 1.0, 1.0);
}
} else {
// And then flash with green if we have a game piece.
- if (status->has_piece) {
+ if (status_flatbuffer->has_piece()) {
SendColors(0.0, 1.0, 0.0);
} else {
SendColors(0.0, 0.0, 0.0);
@@ -130,15 +175,17 @@
}
} else {
line_blink_count_ = 0;
- if (status->has_piece) {
+ if (status_flatbuffer->has_piece()) {
// Green if we have a game piece.
SendColors(0.0, 1.0, 0.0);
- } else if (unsafe_goal->suction.gamepiece_mode == 0 &&
- !status->has_piece) {
+ } else if ((!unsafe_goal->has_suction() ||
+ unsafe_goal->suction()->gamepiece_mode() == 0) &&
+ !status_flatbuffer->has_piece()) {
// Ball mode is orange
SendColors(1.0, 0.1, 0.0);
- } else if (unsafe_goal->suction.gamepiece_mode == 1 &&
- !status->has_piece) {
+ } else if (unsafe_goal->has_suction() &&
+ unsafe_goal->suction()->gamepiece_mode() == 1 &&
+ !status_flatbuffer->has_piece()) {
// Disk mode is deep blue
SendColors(0.05, 0.1, 0.5);
} else {
@@ -146,15 +193,20 @@
}
}
}
+
+ status->Send(status_offset);
}
void Superstructure::SendColors(float red, float green, float blue) {
- auto new_status_light = status_light_sender_.MakeMessage();
- new_status_light->red = red;
- new_status_light->green = green;
- new_status_light->blue = blue;
+ auto builder = status_light_sender_.MakeBuilder();
- if (!new_status_light.Send()) {
+ StatusLight::Builder status_light_builder =
+ builder.MakeBuilder<StatusLight>();
+ status_light_builder.add_red(red);
+ status_light_builder.add_green(green);
+ status_light_builder.add_blue(blue);
+
+ if (!builder.Send(status_light_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send lights.\n");
}
}
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index aab8e7d..f4a23d9 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -2,26 +2,27 @@
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#include "aos/controls/control_loop.h"
-#include "aos/events/event-loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/collision_avoidance.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
#include "y2019/control_loops/superstructure/vacuum.h"
-#include "y2019/status_light.q.h"
+#include "y2019/status_light_generated.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
class Superstructure
- : public ::aos::controls::ControlLoop<SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name =
- ".y2019.control_loops.superstructure.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
@@ -39,16 +40,15 @@
const Vacuum &vacuum() const { return vacuum_; }
protected:
- virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- 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:
void SendColors(float red, float green, float blue);
::aos::Sender<::y2019::StatusLight> status_light_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
PotAndAbsoluteEncoderSubsystem elevator_;
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
deleted file mode 100644
index d19c2b8..0000000
--- a/y2019/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,115 +0,0 @@
-package y2019.control_loops.superstructure;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/profiled_subsystem.q";
-
-struct SuctionGoal {
- // True = apply suction
- bool grab_piece;
-
- // 0 = ball mode
- // 1 = disk mode
-
- int32_t gamepiece_mode;
-};
-
-// Published on ".y2019.control_loops.superstructure.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Meters, 0 = lowest position - mechanical hard stop,
- // positive = upward
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal elevator;
- // 0 = linkage on the sprocket is pointing straight up,
- // positive = forward
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal intake;
- // 0 = Straight up parallel to elevator
- // Positive rotates toward intake from 0
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal wrist;
-
- // Distance stilts extended out of the bottom of the robot. Positive = down.
- // 0 is the height such that the bottom of the stilts is tangent to the
- // bottom of the middle wheels.
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal stilts;
-
- // Positive is rollers intaking inward.
- double roller_voltage;
-
- SuctionGoal suction;
- };
-
- message Status {
- // All subsystems know their location.
- bool zeroed;
-
- // If true, we have aborted. This is the or of all subsystem estops.
- bool estopped;
-
- // Whether suction_pressure indicates cargo is held
- bool has_piece;
-
- // Status of each subsystem.
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
- .frc971.control_loops.AbsoluteEncoderProfiledJointStatus intake;
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
- };
-
- message Position {
- // Input from pressure sensor in bar
- // 1 = 1 atm, 0 = full vacuum
- float suction_pressure;
-
- // Position of the elevator, 0 at lowest position, positive when up.
- .frc971.PotAndAbsolutePosition elevator;
-
- // Position of wrist, 0 when up, positive is rotating toward the front,
- // over the top.
- .frc971.PotAndAbsolutePosition wrist;
-
- // Position of the intake. 0 when rollers are retracted, positive extended.
- .frc971.AbsolutePosition intake_joint;
-
- // Position of the stilts, 0 when retracted (defualt), positive lifts robot.
- .frc971.PotAndAbsolutePosition stilts;
-
- // True if the platform detection sensors detect the platform directly
- // below the robot right behind the left and right wheels. Useful for
- // determining when the robot is all the way on the platform.
- bool platform_left_detect;
- bool platform_right_detect;
- };
-
- message Output {
- // Voltage sent to motors moving elevator up/down. Positive is up.
- double elevator_voltage;
-
- // Voltage sent to wrist motors on elevator to rotate.
- // Positive rotates over the top towards the front of the robot.
- double wrist_voltage;
-
- // Voltage sent to motors on intake joint. Positive extends rollers.
- double intake_joint_voltage;
-
- // Voltage sent to rollers on intake. Positive rolls inward.
- double intake_roller_voltage;
-
- // Voltage sent to motors to move stilts height. Positive moves robot
- // upward.
- double stilts_voltage;
-
- // True opens solenoid (applies suction)
- // Top/bottom are when wrist is toward the front of the robot
- bool intake_suction_top;
- bool intake_suction_bottom;
-
- // Voltage sent to the vacuum pump motors.
- double pump_voltage;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
diff --git a/y2019/control_loops/superstructure/superstructure_goal.fbs b/y2019/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..77e7c8d
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,37 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2019.control_loops.superstructure;
+
+table SuctionGoal {
+ // True = apply suction
+ grab_piece:bool;
+
+ // 0 = ball mode
+ // 1 = disk mode
+
+ gamepiece_mode:int;
+}
+
+table Goal {
+ // Meters, 0 = lowest position - mechanical hard stop,
+ // positive = upward
+ elevator:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ // 0 = linkage on the sprocket is pointing straight up,
+ // positive = forward
+ intake:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ // 0 = Straight up parallel to elevator
+ // Positive rotates toward intake from 0
+ wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+ // Distance stilts extended out of the bottom of the robot. Positive = down.
+ // 0 is the height such that the bottom of the stilts is tangent to the
+ // bottom of the middle wheels.
+ stilts:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+ // Positive is rollers intaking inward.
+ roller_voltage:float;
+
+ suction:SuctionGoal;
+}
+
+root_type Goal;
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index bad771e..7af8ffd 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -4,7 +4,6 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
@@ -27,8 +26,12 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
+using ::frc971::CreateProfileParameters;
using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
@@ -41,15 +44,11 @@
: event_loop_(event_loop),
dt_(dt),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
- superstructure_status_fetcher_(event_loop_->MakeFetcher<
- SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue.status")),
- superstructure_output_fetcher_(event_loop_->MakeFetcher<
- SuperstructureQueue::Output>(
- ".y2019.control_loops.superstructure.superstructure_queue.output")),
+ event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<Output>("/superstructure")),
elevator_plant_(
new CappedTestPlant(::y2019::control_loops::superstructure::
elevator::MakeElevatorPlant())),
@@ -136,16 +135,38 @@
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
- ::aos::Sender<SuperstructureQueue::Position>::Message position =
- superstructure_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
- elevator_pot_encoder_.GetSensorValues(&position->elevator);
- wrist_pot_encoder_.GetSensorValues(&position->wrist);
- intake_pot_encoder_.GetSensorValues(&position->intake_joint);
- stilts_pot_encoder_.GetSensorValues(&position->stilts);
- position->suction_pressure = simulated_pressure_;
+ frc971::PotAndAbsolutePosition::Builder elevator_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> elevator_offset =
+ elevator_pot_encoder_.GetSensorValues(&elevator_builder);
- position.Send();
+ frc971::PotAndAbsolutePosition::Builder wrist_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> wrist_offset =
+ wrist_pot_encoder_.GetSensorValues(&wrist_builder);
+
+ frc971::AbsolutePosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::AbsolutePosition>();
+ flatbuffers::Offset<frc971::AbsolutePosition> intake_offset =
+ intake_pot_encoder_.GetSensorValues(&intake_builder);
+
+ frc971::PotAndAbsolutePosition::Builder stilts_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> stilts_offset =
+ stilts_pot_encoder_.GetSensorValues(&stilts_builder);
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_elevator(elevator_offset);
+ position_builder.add_wrist(wrist_offset);
+ position_builder.add_intake_joint(intake_offset);
+ position_builder.add_stilts(stilts_offset);
+ position_builder.add_suction_pressure(simulated_pressure_);
+
+ builder.Send(position_builder.Finish());
}
double elevator_position() const { return elevator_plant_->X(0, 0); }
@@ -183,58 +204,58 @@
const double voltage_check_elevator =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_status_fetcher_->elevator.state) ==
+ superstructure_status_fetcher_->elevator()->state()) ==
PotAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().elevator.subsystem_params.operating_voltage
: constants::GetValues().elevator.subsystem_params.zeroing_voltage;
const double voltage_check_wrist =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_status_fetcher_->wrist.state) ==
+ superstructure_status_fetcher_->wrist()->state()) ==
PotAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().wrist.subsystem_params.operating_voltage
: constants::GetValues().wrist.subsystem_params.zeroing_voltage;
const double voltage_check_intake =
(static_cast<AbsoluteEncoderSubsystem::State>(
- superstructure_status_fetcher_->intake.state) ==
+ superstructure_status_fetcher_->intake()->state()) ==
AbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().intake.operating_voltage
: constants::GetValues().intake.zeroing_voltage;
const double voltage_check_stilts =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_status_fetcher_->stilts.state) ==
+ superstructure_status_fetcher_->stilts()->state()) ==
PotAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().stilts.subsystem_params.operating_voltage
: constants::GetValues().stilts.subsystem_params.zeroing_voltage;
- EXPECT_NEAR(superstructure_output_fetcher_->elevator_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->elevator_voltage(), 0.0,
voltage_check_elevator);
- EXPECT_NEAR(superstructure_output_fetcher_->wrist_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->wrist_voltage(), 0.0,
voltage_check_wrist);
- EXPECT_NEAR(superstructure_output_fetcher_->intake_joint_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->intake_joint_voltage(), 0.0,
voltage_check_intake);
- EXPECT_NEAR(superstructure_output_fetcher_->stilts_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->stilts_voltage(), 0.0,
voltage_check_stilts);
::Eigen::Matrix<double, 1, 1> elevator_U;
- elevator_U << superstructure_output_fetcher_->elevator_voltage +
+ elevator_U << superstructure_output_fetcher_->elevator_voltage() +
elevator_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> wrist_U;
- wrist_U << superstructure_output_fetcher_->wrist_voltage +
+ wrist_U << superstructure_output_fetcher_->wrist_voltage() +
wrist_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> intake_U;
- intake_U << superstructure_output_fetcher_->intake_joint_voltage +
+ intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
intake_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> stilts_U;
- stilts_U << superstructure_output_fetcher_->stilts_voltage +
+ stilts_U << superstructure_output_fetcher_->stilts_voltage() +
stilts_plant_->voltage_offset();
elevator_plant_->Update(elevator_U);
@@ -330,19 +351,19 @@
}
private:
- void CheckCollisions(const SuperstructureQueue::Status *status) {
- ASSERT_FALSE(
- collision_avoidance_.IsCollided(wrist_position(), elevator_position(),
- intake_position(), status->has_piece));
+ void CheckCollisions(const Status *status) {
+ ASSERT_FALSE(collision_avoidance_.IsCollided(
+ wrist_position(), elevator_position(), intake_position(),
+ status->has_piece()));
}
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
- ::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_;
bool first_ = true;
@@ -381,24 +402,20 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2019/config.json"),
+ chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
- superstructure_goal_fetcher_(test_event_loop_->MakeFetcher<
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
- superstructure_goal_sender_(test_event_loop_->MakeSender<
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
- superstructure_status_fetcher_(test_event_loop_->MakeFetcher<
- SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue.status")),
- superstructure_output_fetcher_(test_event_loop_->MakeFetcher<
- SuperstructureQueue::Output>(
- ".y2019.control_loops.superstructure.superstructure_queue.output")),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
superstructure_position_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop()),
@@ -410,13 +427,13 @@
superstructure_goal_fetcher_.Fetch();
superstructure_status_fetcher_.Fetch();
- EXPECT_NEAR(superstructure_goal_fetcher_->elevator.unsafe_goal,
- superstructure_status_fetcher_->elevator.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->wrist.unsafe_goal,
+ EXPECT_NEAR(superstructure_goal_fetcher_->elevator()->unsafe_goal(),
+ superstructure_status_fetcher_->elevator()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->wrist()->unsafe_goal(),
superstructure_plant_.wrist_position(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.unsafe_goal,
- superstructure_status_fetcher_->intake.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->stilts.unsafe_goal,
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->unsafe_goal(),
+ superstructure_status_fetcher_->intake()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->stilts()->unsafe_goal(),
superstructure_plant_.stilts_position(), 0.001);
}
@@ -428,17 +445,16 @@
superstructure_status_fetcher_.Fetch();
// 2 Seconds
ASSERT_LE(i, 2 * 1.0 / .00505);
- } while (!superstructure_status_fetcher_.get()->zeroed);
+ } while (!superstructure_status_fetcher_.get()->zeroed());
}
::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<SuperstructureQueue::Output> superstructure_output_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Position>
- superstructure_position_fetcher_;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<Position> superstructure_position_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -459,13 +475,29 @@
WaitUntilZeroed();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->elevator.unsafe_goal = 1.4;
- goal->wrist.unsafe_goal = 1.0;
- goal->intake.unsafe_goal = 1.1;
- goal->stilts.unsafe_goal = 0.1;
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
VerifyNearGoal();
@@ -485,24 +517,36 @@
WaitUntilZeroed();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = 1.4;
- goal->elevator.profile_params.max_velocity = 1;
- goal->elevator.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->wrist.unsafe_goal = 1.0;
- goal->wrist.profile_params.max_velocity = 1;
- goal->wrist.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->intake.unsafe_goal = 1.1;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->stilts.unsafe_goal = 0.1;
- goal->stilts.profile_params.max_velocity = 1;
- goal->stilts.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -520,13 +564,33 @@
// Zero it before we move.
WaitUntilZeroed();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
- goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
- goal->stilts.unsafe_goal = constants::Values::kStiltsRange().upper;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kStiltsRange().upper);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
VerifyNearGoal();
@@ -534,24 +598,36 @@
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
- goal->elevator.profile_params.max_velocity = 20.0;
- goal->elevator.profile_params.max_acceleration = 0.1;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
- goal->wrist.profile_params.max_velocity = 20.0;
- goal->wrist.profile_params.max_acceleration = 0.1;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
- goal->intake.profile_params.max_velocity = 20.0;
- goal->intake.profile_params.max_acceleration = 0.1;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
- goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
- goal->stilts.profile_params.max_velocity = 20.0;
- goal->stilts.profile_params.max_acceleration = 0.1;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kStiltsRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_elevator_velocity(23.0);
superstructure_plant_.set_peak_elevator_acceleration(0.2);
@@ -564,36 +640,6 @@
RunFor(chrono::seconds(8));
VerifyNearGoal();
-
- // Now do a high acceleration move with a low velocity limit.
- {
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
- goal->elevator.profile_params.max_velocity = 0.1;
- goal->elevator.profile_params.max_acceleration = 10.0;
-
- goal->wrist.unsafe_goal = constants::Values::kWristRange().lower;
- goal->wrist.profile_params.max_velocity = 0.1;
- goal->wrist.profile_params.max_acceleration = 10.0;
-
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
- goal->intake.profile_params.max_velocity = 0.1;
- goal->intake.profile_params.max_acceleration = 10.0;
-
- goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
- goal->stilts.profile_params.max_velocity = 0.1;
- goal->stilts.profile_params.max_acceleration = 10.0;
- }
- superstructure_plant_.set_peak_elevator_velocity(0.2);
- superstructure_plant_.set_peak_elevator_acceleration(11.0);
- superstructure_plant_.set_peak_wrist_velocity(0.2);
- superstructure_plant_.set_peak_wrist_acceleration(11.0);
- superstructure_plant_.set_peak_intake_velocity(0.2);
- superstructure_plant_.set_peak_intake_acceleration(11.0);
- superstructure_plant_.set_peak_stilts_velocity(0.2);
- superstructure_plant_.set_peak_stilts_acceleration(11.0);
-
- VerifyNearGoal();
}
// Tests if the robot zeroes properly... maybe redundant?
@@ -604,25 +650,36 @@
superstructure_plant_.InitializeStiltsPosition(0.1);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->elevator.unsafe_goal = 1.4;
- goal->elevator.profile_params.max_velocity = 1;
- goal->elevator.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->wrist.unsafe_goal = 1.0;
- goal->wrist.profile_params.max_velocity = 1;
- goal->wrist.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->intake.unsafe_goal = 1.1;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->stilts.unsafe_goal = 0.1;
- goal->stilts.profile_params.max_velocity = 1;
- goal->stilts.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- ASSERT_TRUE(goal.Send());
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitUntilZeroed();
VerifyNearGoal();
@@ -660,13 +717,30 @@
WaitUntilZeroed();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
- goal->wrist.unsafe_goal = -M_PI / 3.0;
- goal->intake.unsafe_goal =
- CollisionAvoidance::kIntakeInAngle - CollisionAvoidance::kEpsIntake;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().lower);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), -M_PI / 3.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), CollisionAvoidance::kIntakeInAngle -
+ CollisionAvoidance::kEpsIntake);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -683,37 +757,71 @@
// Get the elevator and wrist out of the way and set the Intake to where
// we should be able to spin and verify that they do
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
- goal->wrist.unsafe_goal = 0.0;
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
- goal->roller_voltage = 6.0;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_roller_voltage(6.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
superstructure_goal_fetcher_.Fetch();
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage,
- superstructure_goal_fetcher_->roller_voltage);
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(),
+ superstructure_goal_fetcher_->roller_voltage());
VerifyNearGoal();
// Move the intake where we oughtn't to spin the rollers and verify they don't
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
- goal->wrist.unsafe_goal = 0.0;
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
- goal->roller_voltage = 6.0;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_roller_voltage(6.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
superstructure_goal_fetcher_.Fetch();
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(), 0.0);
VerifyNearGoal();
}
@@ -723,10 +831,33 @@
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = true;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), true);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10));
@@ -735,7 +866,7 @@
superstructure_plant_.set_simulated_pressure(0.0);
RunFor(chrono::seconds(2));
superstructure_status_fetcher_.Fetch();
- EXPECT_TRUE(superstructure_status_fetcher_->has_piece);
+ EXPECT_TRUE(superstructure_status_fetcher_->has_piece());
}
// Tests the Vacuum backs off after acquiring a gamepiece
@@ -744,23 +875,47 @@
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = true;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), true);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Verify that at 0 pressure after short time voltage is still high
superstructure_plant_.set_simulated_pressure(0.0);
RunFor(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10));
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->pump_voltage, Vacuum::kPumpVoltage);
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage(),
+ Vacuum::kPumpVoltage);
// Verify that after waiting with a piece the pump voltage goes to the
// has piece voltage
RunFor(chrono::seconds(2));
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->pump_voltage,
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage(),
Vacuum::kPumpHasPieceVoltage);
}
@@ -770,30 +925,77 @@
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = true;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), true);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Get a Gamepiece
superstructure_plant_.set_simulated_pressure(0.0);
RunFor(chrono::seconds(2));
superstructure_status_fetcher_.Fetch();
- EXPECT_TRUE(superstructure_status_fetcher_->has_piece);
+ EXPECT_TRUE(superstructure_status_fetcher_->has_piece());
// Turn off suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = false;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), false);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_simulated_pressure(1.0);
// Run for a short while and check that voltage dropped to 0
RunFor(chrono::milliseconds(10));
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->pump_voltage, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage(), 0.0);
}
// Tests that running disabled, ya know, works
diff --git a/y2019/control_loops/superstructure/superstructure_main.cc b/y2019/control_loops/superstructure/superstructure_main.cc
index 27520c1..f831774 100644
--- a/y2019/control_loops/superstructure/superstructure_main.cc
+++ b/y2019/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2019/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() {
+int main(int /*argc*/, char * /*argv*/ []) {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2019::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2019/control_loops/superstructure/superstructure_output.fbs b/y2019/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..b679347
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,30 @@
+namespace y2019.control_loops.superstructure;
+
+table Output {
+ // Voltage sent to motors moving elevator up/down. Positive is up.
+ elevator_voltage:double;
+
+ // Voltage sent to wrist motors on elevator to rotate.
+ // Positive rotates over the top towards the front of the robot.
+ wrist_voltage:double;
+
+ // Voltage sent to motors on intake joint. Positive extends rollers.
+ intake_joint_voltage:double;
+
+ // Voltage sent to rollers on intake. Positive rolls inward.
+ intake_roller_voltage:double;
+
+ // Voltage sent to motors to move stilts height. Positive moves robot
+ // upward.
+ stilts_voltage:double;
+
+ // True opens solenoid (applies suction)
+ // Top/bottom are when wrist is toward the front of the robot
+ intake_suction_top:bool;
+ intake_suction_bottom:bool;
+
+ // Voltage sent to the vacuum pump motors.
+ pump_voltage:double;
+}
+
+root_type Output;
diff --git a/y2019/control_loops/superstructure/superstructure_position.fbs b/y2019/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..80a851e
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,30 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2019.control_loops.superstructure;
+
+table Position {
+ // Input from pressure sensor in bar
+ // 1 = 1 atm, 0 = full vacuum
+ suction_pressure:float;
+
+ // Position of the elevator, 0 at lowest position, positive when up.
+ elevator:frc971.PotAndAbsolutePosition;
+
+ // Position of wrist, 0 when up, positive is rotating toward the front,
+ // over the top.
+ wrist:frc971.PotAndAbsolutePosition;
+
+ // Position of the intake. 0 when rollers are retracted, positive extended.
+ intake_joint:frc971.AbsolutePosition;
+
+ // Position of the stilts, 0 when retracted (defualt), positive lifts robot.
+ stilts:frc971.PotAndAbsolutePosition;
+
+ // True if the platform detection sensors detect the platform directly
+ // below the robot right behind the left and right wheels. Useful for
+ // determining when the robot is all the way on the platform.
+ platform_left_detect:bool;
+ platform_right_detect:bool;
+}
+
+root_type Position;
diff --git a/y2019/control_loops/superstructure/superstructure_status.fbs b/y2019/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..9d3f44e
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,23 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2019.control_loops.superstructure;
+
+table Status {
+ // All subsystems know their location.
+ zeroed:bool;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ estopped:bool;
+
+ // Whether suction_pressure indicates cargo is held
+ has_piece:bool;
+
+ // Status of each subsystem.
+ elevator:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+ wrist:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+ intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
+ stilts:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+}
+
+root_type Status;
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 956bb79..6d4696b 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -2,7 +2,10 @@
#include <chrono>
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
namespace y2019 {
namespace control_loops {
@@ -16,7 +19,7 @@
constexpr aos::monotonic_clock::duration Vacuum::kReleaseTime;
void Vacuum::Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
- SuperstructureQueue::Output *output, bool *has_piece,
+ OutputT *output, bool *has_piece,
aos::EventLoop *event_loop) {
auto monotonic_now = event_loop->monotonic_now();
bool low_pump_voltage = false;
@@ -46,7 +49,7 @@
low_pump_voltage = *has_piece && filtered_pressure_ < kVacuumOnThreshold;
if (unsafe_goal && output) {
- const bool release = !unsafe_goal->grab_piece;
+ const bool release = !unsafe_goal->grab_piece();
if (release) {
last_release_time_ = monotonic_now;
@@ -58,10 +61,10 @@
output->pump_voltage =
release ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage : kPumpVoltage);
- if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 0) {
+ if (unsafe_goal->grab_piece() && unsafe_goal->gamepiece_mode() == 0) {
output->intake_suction_top = false;
output->intake_suction_bottom = true;
- } else if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 1) {
+ } else if (unsafe_goal->grab_piece() && unsafe_goal->gamepiece_mode() == 1) {
output->intake_suction_top = true;
output->intake_suction_bottom = true;
} else {
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
index e793530..c4671ab 100644
--- a/y2019/control_loops/superstructure/vacuum.h
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -1,8 +1,11 @@
#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
-#include "aos/events/event-loop.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
namespace y2019 {
namespace control_loops {
@@ -12,8 +15,7 @@
public:
Vacuum() {}
void Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
- SuperstructureQueue::Output *output, bool *has_piece,
- aos::EventLoop *event_loop);
+ OutputT *output, bool *has_piece, aos::EventLoop *event_loop);
// Voltage to the vaccum pump when we are attempting to acquire a piece
static constexpr double kPumpVoltage = 8.0;