Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 6a14a5f..4048cdc 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -1,15 +1,42 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "superstructure_queue",
+flatbuffer_cc_library(
+ name = "superstructure_goal_fbs",
srcs = [
- "superstructure.q",
+ "superstructure_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
],
)
@@ -73,8 +100,11 @@
"superstructure_controls.h",
],
deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
":superstructure_plants",
- ":superstructure_queue",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
"//aos/controls:control_loop",
"//aos/util:trapezoid_profile",
@@ -83,7 +113,7 @@
"//frc971/control_loops:state_feedback_loop",
"//frc971/zeroing",
"//y2016:constants",
- "//y2016/queues:ball_detector",
+ "//y2016/queues:ball_detector_fbs",
],
)
@@ -92,11 +122,14 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = ["//y2016:config.json"],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_lib",
- ":superstructure_queue",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
- "//aos:queues",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//aos/time",
@@ -112,8 +145,7 @@
],
deps = [
":superstructure_lib",
- ":superstructure_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 99a130c..8f4d02e 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -1,13 +1,12 @@
#include "y2016/control_loops/superstructure/superstructure.h"
-#include "y2016/control_loops/superstructure/superstructure_controls.h"
#include "aos/commonmath.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "y2016/control_loops/superstructure/integral_intake_plant.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/superstructure/integral_intake_plant.h"
+#include "y2016/control_loops/superstructure/superstructure_controls.h"
+#include "y2016/queues/ball_detector_generated.h"
#include "y2016/constants.h"
@@ -229,11 +228,11 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
collision_avoidance_(&intake_, &arm_) {}
bool Superstructure::IsArmNear(double shoulder_tolerance,
@@ -289,10 +288,10 @@
}
void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) {
+ const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const State state_before_switch = state_;
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
@@ -304,8 +303,8 @@
// Bool to track if we should turn the motors on or not.
bool disable = output == nullptr;
- arm_.Correct(position->shoulder, position->wrist);
- intake_.Correct(position->intake);
+ arm_.Correct(position->shoulder(), position->wrist());
+ intake_.Correct(*position->intake());
// There are 2 main zeroing paths, HIGH_ARM_ZERO and LOW_ARM_ZERO.
//
@@ -571,14 +570,14 @@
4.0, // Shoulder acceleration,
4.0, // Wrist velocity
10.0); // Wrist acceleration.
- intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
- unsafe_goal->max_angular_acceleration_intake);
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(),
+ unsafe_goal->max_angular_acceleration_intake());
requested_shoulder =
- ::std::max(unsafe_goal->angle_shoulder,
+ ::std::max(unsafe_goal->angle_shoulder(),
constants::Values::kShoulderRange.lower);
requested_wrist = 0.0;
- requested_intake = unsafe_goal->angle_intake;
+ requested_intake = unsafe_goal->angle_intake();
// Transition to landing once the profile is close to finished for the
// shoulder.
if (arm_.goal(0, 0) > kShoulderTransitionToLanded + 1e-4 ||
@@ -591,18 +590,18 @@
}
} else {
// Otherwise, give the user what he asked for.
- arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
- unsafe_goal->max_angular_acceleration_shoulder,
- unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_wrist);
- intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
- unsafe_goal->max_angular_acceleration_intake);
+ arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder(),
+ unsafe_goal->max_angular_acceleration_shoulder(),
+ unsafe_goal->max_angular_velocity_wrist(),
+ unsafe_goal->max_angular_acceleration_wrist());
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(),
+ unsafe_goal->max_angular_acceleration_intake());
// Except, don't let the shoulder go all the way down.
- requested_shoulder = ::std::max(unsafe_goal->angle_shoulder,
+ requested_shoulder = ::std::max(unsafe_goal->angle_shoulder(),
kShoulderTransitionToLanded);
- requested_wrist = unsafe_goal->angle_wrist;
- requested_intake = unsafe_goal->angle_intake;
+ requested_wrist = unsafe_goal->angle_wrist();
+ requested_intake = unsafe_goal->angle_intake();
// Transition to landing once the profile is close to finished for the
// shoulder.
@@ -641,9 +640,9 @@
if (unsafe_goal) {
constexpr float kTriggerThreshold = 12.0 * 0.90 / 0.005;
- if (unsafe_goal->voltage_climber > 1.0) {
+ if (unsafe_goal->voltage_climber() > 1.0) {
kill_shoulder_accumulator_ +=
- ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber);
+ ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber());
} else {
kill_shoulder_accumulator_ = 0.0;
}
@@ -673,24 +672,28 @@
}
// Calculate the loops for a cycle.
+ double intake_position_power;
+ double intake_velocity_power;
{
Eigen::Matrix<double, 3, 1> error = intake_.controller().error();
- status->intake.position_power =
+ intake_position_power =
intake_.controller().controller().K(0, 0) * error(0, 0);
- status->intake.velocity_power =
+ intake_velocity_power =
intake_.controller().controller().K(0, 1) * error(1, 0);
}
+ double shoulder_position_power;
+ double shoulder_velocity_power;
+ double wrist_position_power;
+ double wrist_velocity_power;
{
Eigen::Matrix<double, 6, 1> error = arm_.controller().error();
- status->shoulder.position_power =
+ shoulder_position_power =
arm_.controller().controller().K(0, 0) * error(0, 0);
- status->shoulder.velocity_power =
+ shoulder_velocity_power =
arm_.controller().controller().K(0, 1) * error(1, 0);
- status->wrist.position_power =
- arm_.controller().controller().K(0, 2) * error(2, 0);
- status->wrist.velocity_power =
- arm_.controller().controller().K(0, 3) * error(3, 0);
+ wrist_position_power = arm_.controller().controller().K(0, 2) * error(2, 0);
+ wrist_velocity_power = arm_.controller().controller().K(0, 3) * error(3, 0);
}
arm_.Update(disable);
@@ -698,98 +701,135 @@
// Write out all the voltages.
if (output) {
- output->voltage_intake = intake_.voltage();
- output->voltage_shoulder = arm_.shoulder_voltage();
- output->voltage_wrist = arm_.wrist_voltage();
+ OutputT output_struct;
+ output_struct.voltage_intake = intake_.voltage();
+ output_struct.voltage_shoulder = arm_.shoulder_voltage();
+ output_struct.voltage_wrist = arm_.wrist_voltage();
- output->voltage_top_rollers = 0.0;
- output->voltage_bottom_rollers = 0.0;
+ output_struct.voltage_top_rollers = 0.0;
+ output_struct.voltage_bottom_rollers = 0.0;
- output->voltage_climber = 0.0;
- output->unfold_climber = false;
+ output_struct.voltage_climber = 0.0;
+ output_struct.unfold_climber = false;
if (unsafe_goal) {
// Ball detector lights.
ball_detector_fetcher_.Fetch();
bool ball_detected = false;
if (ball_detector_fetcher_.get()) {
- ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ ball_detected = ball_detector_fetcher_->voltage() > 2.5;
}
// Climber.
- output->voltage_climber = ::std::max(
+ output_struct.voltage_climber = ::std::max(
static_cast<float>(0.0),
- ::std::min(unsafe_goal->voltage_climber, kMaxClimberVoltage));
- output->unfold_climber = unsafe_goal->unfold_climber;
+ ::std::min(unsafe_goal->voltage_climber(), kMaxClimberVoltage));
+ output_struct.unfold_climber = unsafe_goal->unfold_climber();
// Intake.
- if (unsafe_goal->force_intake || !ball_detected) {
- output->voltage_top_rollers = ::std::max(
+ if (unsafe_goal->force_intake() || !ball_detected) {
+ output_struct.voltage_top_rollers = ::std::max(
-kMaxIntakeTopVoltage,
- ::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
- output->voltage_bottom_rollers =
+ ::std::min(unsafe_goal->voltage_top_rollers(), kMaxIntakeTopVoltage));
+ output_struct.voltage_bottom_rollers =
::std::max(-kMaxIntakeBottomVoltage,
- ::std::min(unsafe_goal->voltage_bottom_rollers,
+ ::std::min(unsafe_goal->voltage_bottom_rollers(),
kMaxIntakeBottomVoltage));
} else {
- output->voltage_top_rollers = 0.0;
- output->voltage_bottom_rollers = 0.0;
+ output_struct.voltage_top_rollers = 0.0;
+ output_struct.voltage_bottom_rollers = 0.0;
}
// Traverse.
- output->traverse_unlatched = unsafe_goal->traverse_unlatched;
- output->traverse_down = unsafe_goal->traverse_down;
+ output_struct.traverse_unlatched = unsafe_goal->traverse_unlatched();
+ output_struct.traverse_down = unsafe_goal->traverse_down();
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
// Save debug/internal state.
- status->zeroed = arm_.zeroed() && intake_.zeroed();
+ flatbuffers::Offset<frc971::EstimatorState> shoulder_estimator_state_offset =
+ arm_.EstimatorState(status->fbb(), 0);
- status->shoulder.angle = arm_.X_hat(0, 0);
- status->shoulder.angular_velocity = arm_.X_hat(1, 0);
- status->shoulder.goal_angle = arm_.goal(0, 0);
- status->shoulder.goal_angular_velocity = arm_.goal(1, 0);
- status->shoulder.unprofiled_goal_angle = arm_.unprofiled_goal(0, 0);
- status->shoulder.unprofiled_goal_angular_velocity =
- arm_.unprofiled_goal(1, 0);
- status->shoulder.voltage_error = arm_.X_hat(4, 0);
- status->shoulder.calculated_velocity =
- (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
- status->shoulder.estimator_state = arm_.EstimatorState(0);
+ JointState::Builder shoulder_builder = status->MakeBuilder<JointState>();
- status->wrist.angle = arm_.X_hat(2, 0);
- status->wrist.angular_velocity = arm_.X_hat(3, 0);
- status->wrist.goal_angle = arm_.goal(2, 0);
- status->wrist.goal_angular_velocity = arm_.goal(3, 0);
- status->wrist.unprofiled_goal_angle = arm_.unprofiled_goal(2, 0);
- status->wrist.unprofiled_goal_angular_velocity = arm_.unprofiled_goal(3, 0);
- status->wrist.voltage_error = arm_.X_hat(5, 0);
- status->wrist.calculated_velocity =
- (arm_.wrist_angle() - last_wrist_angle_) / 0.005;
- status->wrist.estimator_state = arm_.EstimatorState(1);
+ shoulder_builder.add_angle(arm_.X_hat(0, 0));
+ shoulder_builder.add_angular_velocity(arm_.X_hat(1, 0));
+ shoulder_builder.add_goal_angle(arm_.goal(0, 0));
+ shoulder_builder.add_goal_angular_velocity(arm_.goal(1, 0));
+ shoulder_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(0, 0));
+ shoulder_builder.add_unprofiled_goal_angular_velocity(
+ arm_.unprofiled_goal(1, 0));
+ shoulder_builder.add_voltage_error(arm_.X_hat(4, 0));
+ shoulder_builder.add_calculated_velocity(
+ (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005);
+ shoulder_builder.add_position_power(shoulder_position_power);
+ shoulder_builder.add_velocity_power(shoulder_velocity_power);
+ shoulder_builder.add_estimator_state(shoulder_estimator_state_offset);
- status->intake.angle = intake_.X_hat(0, 0);
- status->intake.angular_velocity = intake_.X_hat(1, 0);
- status->intake.goal_angle = intake_.goal(0, 0);
- status->intake.goal_angular_velocity = intake_.goal(1, 0);
- status->intake.unprofiled_goal_angle = intake_.unprofiled_goal(0, 0);
- status->intake.unprofiled_goal_angular_velocity =
- intake_.unprofiled_goal(1, 0);
- status->intake.calculated_velocity =
- (intake_.position() - last_intake_angle_) / 0.005;
- status->intake.voltage_error = intake_.X_hat(2, 0);
- status->intake.estimator_state = intake_.EstimatorState(0);
- status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
+ flatbuffers::Offset<JointState> shoulder_offset = shoulder_builder.Finish();
- status->shoulder_controller_index = arm_.controller_index();
+ flatbuffers::Offset<frc971::EstimatorState> wrist_estimator_state_offset =
+ arm_.EstimatorState(status->fbb(), 1);
+
+ JointState::Builder wrist_builder = status->MakeBuilder<JointState>();
+
+ wrist_builder.add_angle(arm_.X_hat(2, 0));
+ wrist_builder.add_angular_velocity(arm_.X_hat(3, 0));
+ wrist_builder.add_goal_angle(arm_.goal(2, 0));
+ wrist_builder.add_goal_angular_velocity(arm_.goal(3, 0));
+ wrist_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(2, 0));
+ wrist_builder.add_unprofiled_goal_angular_velocity(
+ arm_.unprofiled_goal(3, 0));
+ wrist_builder.add_voltage_error(arm_.X_hat(5, 0));
+ wrist_builder.add_calculated_velocity(
+ (arm_.wrist_angle() - last_wrist_angle_) / 0.005);
+ wrist_builder.add_position_power(wrist_position_power);
+ wrist_builder.add_velocity_power(wrist_velocity_power);
+ wrist_builder.add_estimator_state(wrist_estimator_state_offset);
+
+ flatbuffers::Offset<JointState> wrist_offset = wrist_builder.Finish();
+
+ flatbuffers::Offset<frc971::EstimatorState> intake_estimator_state_offset =
+ intake_.EstimatorState(status->fbb(), 0);
+
+ JointState::Builder intake_builder = status->MakeBuilder<JointState>();
+ intake_builder.add_position_power(intake_position_power);
+ intake_builder.add_velocity_power(intake_velocity_power);
+ intake_builder.add_angle(intake_.X_hat(0, 0));
+ intake_builder.add_angular_velocity(intake_.X_hat(1, 0));
+ intake_builder.add_goal_angle(intake_.goal(0, 0));
+ intake_builder.add_goal_angular_velocity(intake_.goal(1, 0));
+ intake_builder.add_unprofiled_goal_angle(intake_.unprofiled_goal(0, 0));
+ intake_builder.add_unprofiled_goal_angular_velocity(
+ intake_.unprofiled_goal(1, 0));
+ intake_builder.add_calculated_velocity(
+ (intake_.position() - last_intake_angle_) / 0.005);
+ intake_builder.add_voltage_error(intake_.X_hat(2, 0));
+ intake_builder.add_estimator_state(intake_estimator_state_offset);
+ intake_builder.add_feedforwards_power(intake_.controller().ff_U(0, 0));
+
+ flatbuffers::Offset<JointState> intake_offset = intake_builder.Finish();
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_shoulder(shoulder_offset);
+ status_builder.add_wrist(wrist_offset);
+ status_builder.add_intake(intake_offset);
+
+ status_builder.add_zeroed(arm_.zeroed() && intake_.zeroed());
+ status_builder.add_shoulder_controller_index(arm_.controller_index());
last_shoulder_angle_ = arm_.shoulder_angle();
last_wrist_angle_ = arm_.wrist_angle();
last_intake_angle_ = intake_.position();
- status->estopped = (state_ == ESTOP);
+ status_builder.add_estopped((state_ == ESTOP));
- status->state = state_;
- status->is_collided = is_collided;
+ status_builder.add_state(state_);
+ status_builder.add_is_collided(is_collided);
+
+ status->Send(status_builder.Finish());
last_state_ = state_before_switch;
}
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 610db8e..2219c95 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -8,9 +8,12 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/control_loops/superstructure/superstructure_controls.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
namespace y2016 {
namespace control_loops {
@@ -105,11 +108,10 @@
};
class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2016.control_loops.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
static constexpr double kZeroingVoltage = 6.0;
static constexpr double kShooterHangingVoltage = 6.0;
@@ -209,11 +211,9 @@
bool collided() const { return collision_avoidance_.collided(); }
protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) override;
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
deleted file mode 100644
index d784929..0000000
--- a/y2016/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,145 +0,0 @@
-package y2016.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct JointState {
- // Angle of the joint in radians.
- float angle;
- // Angular velocity of the joint in radians/second.
- float angular_velocity;
- // Profiled goal angle of the joint in radians.
- float goal_angle;
- // Profiled goal angular velocity of the joint in radians/second.
- float goal_angular_velocity;
- // Unprofiled goal angle of the joint in radians.
- float unprofiled_goal_angle;
- // Unprofiled goal angular velocity of the joint in radians/second.
- float unprofiled_goal_angular_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.EstimatorState estimator_state;
-};
-
-// Published on ".y2016.control_loops.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Zero on the intake is when the horizontal tube stock members are level
- // with the top frame rails of the robot. This will be essentially when we
- // are in the intaking position. Positive is up. The angle is measured
- // relative to the top
- // of the robot frame.
- // Zero on the shoulder is when the shoulder is down against the hard stop
- // blocks. Positive is up. The angle is measured relative to the top of
- // the robot frame.
- // Zero on the wrist is horizontal and landed in the bellypan. Positive is
- // the same direction as the shoulder. The angle is measured relative to
- // the top of the robot frame. For calibration, 0 is measured as parallel
- // to the big frame supporting the shooter.
-
- // Goal angles and angular velocities of the superstructure subsystems.
- double angle_intake;
- double angle_shoulder;
- // In relation to the ground plane.
- double angle_wrist;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- float max_angular_velocity_intake;
- float max_angular_velocity_shoulder;
- float max_angular_velocity_wrist;
-
- float max_angular_acceleration_intake;
- float max_angular_acceleration_shoulder;
- float max_angular_acceleration_wrist;
-
- // Voltage to send to the rollers. Positive is sucking in.
- float voltage_top_rollers;
- float voltage_bottom_rollers;
-
- // Voltage to sent to the climber. Positive is pulling the robot up.
- float voltage_climber;
- // If true, unlatch the climber and allow it to unfold.
- bool unfold_climber;
-
- bool force_intake;
-
- // If true, release the latch which holds the traverse mechanism in the
- // middle.
- bool traverse_unlatched;
- // If true, fire the traverse mechanism down.
- bool traverse_down;
- };
-
- message Status {
- // Are the superstructure subsystems zeroed?
- bool zeroed;
-
- // If true, we have aborted.
- bool estopped;
-
- // The internal state of the state machine.
- int32_t state;
-
-
- // Estimated angles and angular velocities of the superstructure subsystems.
- JointState intake;
- JointState shoulder;
- JointState wrist;
-
- int32_t shoulder_controller_index;
-
- // Is the superstructure collided?
- bool is_collided;
- };
-
- message Position {
- // Zero for the intake potentiometer value is horizontal, and positive is
- // up.
- // Zero for the shoulder potentiometer value is horizontal, and positive is
- // up.
- // Zero for the wrist potentiometer value is parallel to the arm and with
- // the shooter wheels pointed towards the shoulder joint. This is measured
- // relative to the arm, not the ground, not like the world we actually
- // present to users.
- .frc971.PotAndIndexPosition intake;
- .frc971.PotAndIndexPosition shoulder;
- .frc971.PotAndIndexPosition wrist;
- };
-
- message Output {
- float voltage_intake;
- float voltage_shoulder;
- float voltage_wrist;
-
- float voltage_top_rollers;
- float voltage_bottom_rollers;
-
- // Voltage to sent to the climber. Positive is pulling the robot up.
- float voltage_climber;
- // If true, release the latch to trigger the climber to unfold.
- bool unfold_climber;
-
- // If true, release the latch to hold the traverse mechanism in the middle.
- bool traverse_unlatched;
- // If true, fire the traverse mechanism down.
- bool traverse_down;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index b8f3a48..269089b 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -1,6 +1,5 @@
#include "y2016/control_loops/superstructure/superstructure_controls.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
#include "y2016/control_loops/superstructure/integral_intake_plant.h"
@@ -96,10 +95,10 @@
// TODO(austin): Handle zeroing errors.
-void Arm::Correct(PotAndIndexPosition position_shoulder,
- PotAndIndexPosition position_wrist) {
- estimators_[kShoulderIndex].UpdateEstimate(position_shoulder);
- estimators_[kWristIndex].UpdateEstimate(position_wrist);
+void Arm::Correct(const PotAndIndexPosition *position_shoulder,
+ const PotAndIndexPosition *position_wrist) {
+ estimators_[kShoulderIndex].UpdateEstimate(*position_shoulder);
+ estimators_[kWristIndex].UpdateEstimate(*position_wrist);
// Handle zeroing errors
if (estimators_[kShoulderIndex].error()) {
@@ -130,7 +129,7 @@
}
{
- Y_ << position_shoulder.encoder, position_wrist.encoder;
+ Y_ << position_shoulder->encoder(), position_wrist->encoder();
Y_ += offset_;
loop_->Correct(Y_);
}
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 8936650..d301054 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -11,7 +11,7 @@
#include "frc971/zeroing/zeroing.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
namespace y2016 {
namespace control_loops {
@@ -111,8 +111,8 @@
Arm();
// Updates our estimator with the latest position.
- void Correct(::frc971::PotAndIndexPosition position_shoulder,
- ::frc971::PotAndIndexPosition position_wrist);
+ void Correct(const ::frc971::PotAndIndexPosition *position_shoulder,
+ const ::frc971::PotAndIndexPosition *position_wrist);
// Forces the goal to be the provided goal.
void ForceGoal(double unprofiled_goal_shoulder, double unprofiled_goal_wrist);
diff --git a/y2016/control_loops/superstructure/superstructure_goal.fbs b/y2016/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..4274bd8
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,50 @@
+namespace y2016.control_loops.superstructure;
+
+table Goal {
+ // Zero on the intake is when the horizontal tube stock members are level
+ // with the top frame rails of the robot. This will be essentially when we
+ // are in the intaking position. Positive is up. The angle is measured
+ // relative to the top
+ // of the robot frame.
+ // Zero on the shoulder is when the shoulder is down against the hard stop
+ // blocks. Positive is up. The angle is measured relative to the top of
+ // the robot frame.
+ // Zero on the wrist is horizontal and landed in the bellypan. Positive is
+ // the same direction as the shoulder. The angle is measured relative to
+ // the top of the robot frame. For calibration, 0 is measured as parallel
+ // to the big frame supporting the shooter.
+
+ // Goal angles and angular velocities of the superstructure subsystems.
+ angle_intake:double;
+ angle_shoulder:double;
+ // In relation to the ground plane.
+ angle_wrist:double;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ max_angular_velocity_intake:float;
+ max_angular_velocity_shoulder:float;
+ max_angular_velocity_wrist:float;
+
+ max_angular_acceleration_intake:float;
+ max_angular_acceleration_shoulder:float;
+ max_angular_acceleration_wrist:float;
+
+ // Voltage to send to the rollers. Positive is sucking in.
+ voltage_top_rollers:float;
+ voltage_bottom_rollers:float;
+
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ voltage_climber:float;
+ // If true, unlatch the climber and allow it to unfold.
+ unfold_climber:bool;
+
+ force_intake:bool;
+
+ // If true, release the latch which holds the traverse mechanism in the
+ // middle.
+ traverse_unlatched:bool;
+ // If true, fire the traverse mechanism down.
+ traverse_down:bool;
+}
+
+root_type Goal;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 79b4d24..d530cb3 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -7,14 +7,16 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
#include "aos/time/time.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2016/control_loops/superstructure/arm_plant.h"
#include "y2016/control_loops/superstructure/intake_plant.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
#include "y2016/constants.h"
@@ -85,14 +87,11 @@
: event_loop_(event_loop),
dt_(dt),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2016.control_loops.superstructure_queue.position")),
+ event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2016.control_loops.superstructure_queue.output")),
+ event_loop_->MakeFetcher<Output>("/superstructure")),
intake_plant_(new IntakePlant(MakeIntakePlant())),
arm_plant_(new ArmPlant(MakeArmPlant())),
pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
@@ -144,14 +143,32 @@
// Sends a queue message with the position.
void SendPositionMessage() {
- ::aos::Sender<control_loops::SuperstructureQueue::Position>::Message
- position = superstructure_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
- pot_encoder_intake_.GetSensorValues(&position->intake);
- pot_encoder_shoulder_.GetSensorValues(&position->shoulder);
- pot_encoder_wrist_.GetSensorValues(&position->wrist);
+ frc971::PotAndIndexPosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
- position.Send();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> intake_offset =
+ pot_encoder_intake_.GetSensorValues(&intake_builder);
+
+ frc971::PotAndIndexPosition::Builder shoulder_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> shoulder_offset =
+ pot_encoder_shoulder_.GetSensorValues(&shoulder_builder);
+
+ frc971::PotAndIndexPosition::Builder wrist_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> wrist_offset =
+ pot_encoder_wrist_.GetSensorValues(&wrist_builder);
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_intake(intake_offset);
+ position_builder.add_shoulder(shoulder_offset);
+ position_builder.add_wrist(wrist_offset);
+
+ builder.Send(position_builder.Finish());
}
double shoulder_angle() const { return arm_plant_->X(0, 0); }
@@ -180,37 +197,39 @@
// Feed voltages into physics simulation.
::Eigen::Matrix<double, 1, 1> intake_U;
::Eigen::Matrix<double, 2, 1> arm_U;
- intake_U << superstructure_output_fetcher_->voltage_intake +
+ intake_U << superstructure_output_fetcher_->voltage_intake() +
intake_plant_->voltage_offset();
- arm_U << superstructure_output_fetcher_->voltage_shoulder +
+ arm_U << superstructure_output_fetcher_->voltage_shoulder() +
arm_plant_->shoulder_voltage_offset(),
- superstructure_output_fetcher_->voltage_wrist +
+ superstructure_output_fetcher_->voltage_wrist() +
arm_plant_->wrist_voltage_offset();
// Verify that the correct power limits are being respected depending on
// which mode we are in.
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
- if (superstructure_status_fetcher_->state == Superstructure::RUNNING ||
- superstructure_status_fetcher_->state ==
+ if (superstructure_status_fetcher_->state() == Superstructure::RUNNING ||
+ superstructure_status_fetcher_->state() ==
Superstructure::LANDING_RUNNING) {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ AOS_CHECK_LE(
+ ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kOperatingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
Superstructure::kOperatingVoltage);
} else {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ AOS_CHECK_LE(
+ ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kZeroingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
Superstructure::kZeroingVoltage);
}
if (arm_plant_->X(0, 0) <=
Superstructure::kShoulderTransitionToLanded + 1e-4) {
- AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder,
+ AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder(),
Superstructure::kLandingShoulderDownVoltage - 0.00001);
}
@@ -309,9 +328,9 @@
bool first_ = true;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
::std::unique_ptr<IntakePlant> intake_plant_;
::std::unique_ptr<ArmPlant> arm_plant_;
@@ -334,17 +353,16 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2016/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
superstructure_goal_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
superstructure_goal_sender_(
- test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
superstructure_status_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop()),
@@ -357,26 +375,26 @@
EXPECT_TRUE(superstructure_goal_fetcher_.get() != nullptr);
EXPECT_TRUE(superstructure_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
- superstructure_status_fetcher_->intake.angle, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake(),
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder(),
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist(),
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake(),
superstructure_plant_.intake_angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder(),
superstructure_plant_.shoulder_angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist(),
superstructure_plant_.wrist_angle(), 0.001);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -390,17 +408,18 @@
TEST_F(SuperstructureTest, DoesNothing) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0;
- message->angle_shoulder = 0;
- message->angle_wrist = 0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0);
+ goal_builder.add_angle_shoulder(0);
+ goal_builder.add_angle_wrist(0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -412,17 +431,18 @@
SetEnabled(true);
// Set a reasonable goal.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI / 4.0;
- message->angle_shoulder = 1.4;
- message->angle_wrist = M_PI / 4.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI / 4.0);
+ goal_builder.add_angle_shoulder(1.4);
+ goal_builder.add_angle_wrist(M_PI / 4.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -437,43 +457,45 @@
SetEnabled(true);
// Set some ridiculous goals to test upper limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI * 10;
- message->angle_shoulder = M_PI * 10;
- message->angle_wrist = M_PI * 10;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI * 10);
+ goal_builder.add_angle_shoulder(M_PI * 10);
+ goal_builder.add_angle_wrist(M_PI * 10);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kWristRange.upper +
constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
// Set some ridiculous goals to test limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI * 10;
- message->angle_shoulder = M_PI * 10;
- message->angle_wrist = -M_PI * 10.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI * 10);
+ goal_builder.add_angle_shoulder(M_PI * 10);
+ goal_builder.add_angle_wrist(-M_PI * 10.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -481,26 +503,27 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kWristRange.lower +
constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
// Set some ridiculous goals to test lower limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = -M_PI * 10;
- message->angle_shoulder = -M_PI * 10;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(-M_PI * 10);
+ goal_builder.add_angle_shoulder(-M_PI * 10);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -508,27 +531,28 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.lower,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist()->angle(), 0.001);
}
// Tests that the loop zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -557,12 +581,13 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.lower);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = constants::Values::kWristRange.upper +
- constants::Values::kShoulderRange.upper;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.upper);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.upper +
+ constants::Values::kShoulderRange.upper);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// We have to wait for it to put the elevator in a safe position as well.
RunFor(chrono::seconds(15));
@@ -580,11 +605,12 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// We have to wait for it to put the superstructure in a safe position as
// well.
@@ -604,11 +630,12 @@
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower + 0.3;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower + 0.3);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
@@ -628,11 +655,13 @@
superstructure_plant_.set_check_for_collisions(false);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower + 0.03;
- message->angle_shoulder = constants::Values::kShoulderRange.lower + 0.03;
- message->angle_wrist = constants::Values::kWristRange.lower + 0.03;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower + 0.03);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower +
+ 0.03);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.lower + 0.03);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(100));
@@ -659,17 +688,18 @@
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = constants::Values::kWristRange.upper;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.upper);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.upper);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Expected states to cycle through and check in order.
@@ -730,17 +760,18 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = constants::Values::kWristRange.lower;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.lower);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Expected states to cycle through and check in order.
@@ -812,11 +843,12 @@
superstructure_plant_.InitializeRelativeWristPosition(0.0);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
@@ -834,12 +866,13 @@
superstructure_plant_.InitializeRelativeWristPosition(-0.001);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- constants::Values::kShoulderEncoderIndexDifference * 10;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ constants::Values::kShoulderEncoderIndexDifference * 10);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Run disabled for 2 seconds
@@ -883,17 +916,18 @@
TEST_F(SuperstructureTest, ShoulderAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -902,17 +936,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.5;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.5);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// TODO(austin): The profile isn't feasible, so when we try to track it, we
@@ -930,17 +965,18 @@
TEST_F(SuperstructureTest, IntakeAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -949,17 +985,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.5;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.5);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_acceleration(1.20);
@@ -974,18 +1011,19 @@
TEST_F(SuperstructureTest, WristAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -994,18 +1032,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.5;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.5);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_acceleration(1.05);
@@ -1021,18 +1060,19 @@
TEST_F(SuperstructureTest, SaturatedIntakeProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1041,18 +1081,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.5;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 4.5;
- message->max_angular_acceleration_intake = 800;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 100;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 100;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.5);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(4.5);
+ goal_builder.add_max_angular_acceleration_intake(800);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(100);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(100);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(4.65);
@@ -1068,17 +1109,18 @@
TEST_F(SuperstructureTest, SaturatedShoulderProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1087,17 +1129,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.9;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1.0;
- message->max_angular_acceleration_intake = 1.0;
- message->max_angular_velocity_shoulder = 5.0;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 100;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.9);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1.0);
+ goal_builder.add_max_angular_acceleration_intake(1.0);
+ goal_builder.add_max_angular_velocity_shoulder(5.0);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(100);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(1.0);
@@ -1113,18 +1156,19 @@
TEST_F(SuperstructureTest, SaturatedWristProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1133,18 +1177,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 1.3;
- message->max_angular_velocity_intake = 1.0;
- message->max_angular_acceleration_intake = 1.0;
- message->max_angular_velocity_shoulder = 1.0;
- message->max_angular_acceleration_shoulder = 1.0;
- message->max_angular_velocity_wrist = 10.0;
- message->max_angular_acceleration_wrist = 160.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(1.3);
+ goal_builder.add_max_angular_velocity_intake(1.0);
+ goal_builder.add_max_angular_acceleration_intake(1.0);
+ goal_builder.add_max_angular_velocity_shoulder(1.0);
+ goal_builder.add_max_angular_acceleration_shoulder(1.0);
+ goal_builder.add_max_angular_velocity_wrist(10.0);
+ goal_builder.add_max_angular_acceleration_wrist(160.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(1.0);
@@ -1165,21 +1210,26 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = constants::Values::kShoulderRange.lower; // Down
- message->angle_wrist = 0.0; // Stowed
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(
+ constants::Values::kShoulderRange.lower); // Down
+ goal_builder.add_angle_wrist(0.0); // Stowed
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = M_PI / 4.0; // in the collision area
- message->angle_wrist = M_PI / 2.0; // down
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(M_PI / 4.0); // in the collision area
+ goal_builder.add_angle_wrist(M_PI / 2.0); // down
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1188,27 +1238,30 @@
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be out of the way despite being told to move to stowing.
- EXPECT_LT(superstructure_status_fetcher_->intake.angle, M_PI);
- EXPECT_LT(superstructure_status_fetcher_->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(), M_PI);
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(),
constants::Values::kIntakeRange.upper);
- EXPECT_LT(superstructure_status_fetcher_->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(),
CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
// The arm should have reached its goal.
- EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder()->angle(),
+ 0.001);
// The wrist should be forced into a stowing position.
// Since the intake is kicked out, we can be within
// kMaxWristAngleForMovingByIntake
- EXPECT_NEAR(0, superstructure_status_fetcher_->wrist.angle,
+ EXPECT_NEAR(0, superstructure_status_fetcher_->wrist()->angle(),
CollisionAvoidance::kMaxWristAngleForMovingByIntake + 0.001);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = M_PI / 2.0; // in the collision area
- message->angle_wrist = M_PI; // forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(M_PI / 2.0); // in the collision area
+ goal_builder.add_angle_wrist(M_PI); // forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1224,11 +1277,12 @@
superstructure_plant_.InitializeAbsoluteWristPosition(M_PI); // forward
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = M_PI; // intentionally asking for forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(M_PI); // intentionally asking for forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
@@ -1237,12 +1291,12 @@
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be in intaking position, as asked.
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake()->angle(), 0.001);
// The shoulder and wrist should both be at zero degrees (i.e.
// stowed/intaking position).
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist()->angle(), 0.001);
}
// Make sure that we can properly detect a collision.
@@ -1250,11 +1304,12 @@
SetEnabled(true);
// Zero & go straight up with the shoulder.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI * 0.5;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI * 0.5);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1290,11 +1345,12 @@
SetEnabled(true);
// Zero & go straight up with the shoulder.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1333,11 +1389,12 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0; // intentionally asking for forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0); // intentionally asking for forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1353,7 +1410,7 @@
RunFor(chrono::seconds(2));
superstructure_goal_fetcher_.Fetch();
EXPECT_LE(constants::Values::kShoulderRange.lower,
- superstructure_goal_fetcher_->angle_shoulder);
+ superstructure_goal_fetcher_->angle_shoulder());
}
// Make sure that we land slowly.
@@ -1361,27 +1418,29 @@
SetEnabled(true);
// Zero & go to initial position.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI * 0.25;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI * 0.25);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
// Tell it to land in the bellypan as fast as possible.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Wait until we hit the transition point.
@@ -1400,27 +1459,29 @@
SetEnabled(true);
// Zero & go to initial position.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
// Tell it to take off as fast as possible.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI / 2.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI / 2.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Wait until we hit the transition point.
diff --git a/y2016/control_loops/superstructure/superstructure_main.cc b/y2016/control_loops/superstructure/superstructure_main.cc
index 00fa7dd..abe2ec9 100644
--- a/y2016/control_loops/superstructure/superstructure_main.cc
+++ b/y2016/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2016/control_loops/superstructure/superstructure.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2016/control_loops/superstructure/superstructure_output.fbs b/y2016/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..40a0091
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,22 @@
+namespace y2016.control_loops.superstructure;
+
+table Output {
+ voltage_intake:float;
+ voltage_shoulder:float;
+ voltage_wrist:float;
+
+ voltage_top_rollers:float;
+ voltage_bottom_rollers:float;
+
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ voltage_climber:float;
+ // If true, release the latch to trigger the climber to unfold.
+ unfold_climber:bool;
+
+ // If true, release the latch to hold the traverse mechanism in the middle.
+ traverse_unlatched:bool;
+ // If true, fire the traverse mechanism down.
+ traverse_down:bool;
+}
+
+root_type Output;
diff --git a/y2016/control_loops/superstructure/superstructure_position.fbs b/y2016/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..fb356e0
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,19 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2016.control_loops.superstructure;
+
+table Position {
+ // Zero for the intake potentiometer value is horizontal, and positive is
+ // up.
+ // Zero for the shoulder potentiometer value is horizontal, and positive is
+ // up.
+ // Zero for the wrist potentiometer value is parallel to the arm and with
+ // the shooter wheels pointed towards the shoulder joint. This is measured
+ // relative to the arm, not the ground, not like the world we actually
+ // present to users.
+ intake:frc971.PotAndIndexPosition;
+ shoulder:frc971.PotAndIndexPosition;
+ wrist:frc971.PotAndIndexPosition;
+}
+
+root_type Position;
diff --git a/y2016/control_loops/superstructure/superstructure_status.fbs b/y2016/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..373bfe2
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,56 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2016.control_loops.superstructure;
+
+table JointState {
+ // Angle of the joint in radians.
+ angle:float;
+ // Angular velocity of the joint in radians/second.
+ angular_velocity:float;
+ // Profiled goal angle of the joint in radians.
+ goal_angle:float;
+ // Profiled goal angular velocity of the joint in radians/second.
+ goal_angular_velocity:float;
+ // Unprofiled goal angle of the joint in radians.
+ unprofiled_goal_angle:float;
+ // Unprofiled goal angular velocity of the joint in radians/second.
+ unprofiled_goal_angular_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.EstimatorState;
+}
+
+table Status {
+ // Are the superstructure subsystems zeroed?
+ zeroed:bool;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // The internal state of the state machine.
+ state:int;
+
+
+ // Estimated angles and angular velocities of the superstructure subsystems.
+ intake:JointState;
+ shoulder:JointState;
+ wrist:JointState;
+
+ shoulder_controller_index:int;
+
+ // Is the superstructure collided?
+ is_collided:bool;
+}
+
+root_type Status;