Simplified intake portions of superstructure queue.
Deleted IntakeOuput and IntakePosition as they are not needed.
Change-Id: Ib3e045596a7f9246f300a527d94ef7ab79e47b95
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 1c5a4e5..2cddf83 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -37,15 +37,15 @@
intake_left_.Iterate(unsafe_goal != nullptr
? &(unsafe_goal->intake.left_intake_angle)
: nullptr,
- &(position->intake.left),
- output != nullptr ? &(output->intake.left) : nullptr,
+ &(position->left_intake),
+ output != nullptr ? &(output->left_intake) : nullptr,
&(status->left_intake));
intake_right_.Iterate(unsafe_goal != nullptr
? &(unsafe_goal->intake.right_intake_angle)
: nullptr,
- &(position->intake.right),
- output != nullptr ? &(output->intake.right) : nullptr,
+ &(position->right_intake),
+ output != nullptr ? &(output->right_intake) : nullptr,
&(status->right_intake));
arm_.Iterate(
@@ -68,18 +68,18 @@
kMaxIntakeRollerVoltage));
constexpr int kReverseTime = 15;
if (unsafe_goal->intake.roller_voltage < 0.0) {
- output->intake.left.voltage_rollers = roller_voltage;
- output->intake.right.voltage_rollers = roller_voltage;
+ output->left_intake.voltage_rollers = roller_voltage;
+ output->right_intake.voltage_rollers = roller_voltage;
rotation_state_ = RotationState::NOT_ROTATING;
rotation_count_ = 0;
} else {
switch (rotation_state_) {
case RotationState::NOT_ROTATING:
- if (position->intake.left.beam_break) {
+ if (position->left_intake.beam_break) {
rotation_state_ = RotationState::ROTATING_RIGHT;
rotation_count_ = kReverseTime;
break;
- } else if (position->intake.right.beam_break) {
+ } else if (position->right_intake.beam_break) {
rotation_state_ = RotationState::ROTATING_LEFT;
rotation_count_ = kReverseTime;
break;
@@ -87,7 +87,7 @@
break;
}
case RotationState::ROTATING_LEFT:
- if (position->intake.right.beam_break) {
+ if (position->right_intake.beam_break) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -97,7 +97,7 @@
}
break;
case RotationState::ROTATING_RIGHT:
- if (position->intake.left.beam_break) {
+ if (position->left_intake.beam_break) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -110,16 +110,16 @@
switch (rotation_state_) {
case RotationState::NOT_ROTATING:
- output->intake.left.voltage_rollers = roller_voltage;
- output->intake.right.voltage_rollers = roller_voltage;
+ output->left_intake.voltage_rollers = roller_voltage;
+ output->right_intake.voltage_rollers = roller_voltage;
break;
case RotationState::ROTATING_LEFT:
- output->intake.left.voltage_rollers = roller_voltage;
- output->intake.right.voltage_rollers = -roller_voltage;
+ output->left_intake.voltage_rollers = roller_voltage;
+ output->right_intake.voltage_rollers = -roller_voltage;
break;
case RotationState::ROTATING_RIGHT:
- output->intake.left.voltage_rollers = -roller_voltage;
- output->intake.right.voltage_rollers = roller_voltage;
+ output->left_intake.voltage_rollers = -roller_voltage;
+ output->right_intake.voltage_rollers = roller_voltage;
break;
}
}
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 93d5935..75198fa 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -60,16 +60,6 @@
bool beam_break;
};
-struct IntakePosition {
- // Values of the series elastic encoders on the left side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors left;
-
- // Values of the series elastic encoders on the right side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors right;
-};
-
struct ArmStatus {
// State of the estimators.
.frc971.AbsoluteEstimatorState proximal_estimator_state;
@@ -128,15 +118,6 @@
double voltage_rollers;
};
-struct IntakeOutput {
- // Voltage sent to the parts on the left side of the intake.
- IntakeVoltage left;
-
- // Voltage sent to the parts on the right side of the intake.
- IntakeVoltage right;
-};
-
-
queue_group SuperstructureQueue {
implements aos.control_loops.ControlLoop;
@@ -166,7 +147,14 @@
};
message Position {
- IntakePosition intake;
+ // Values of the series elastic encoders on the left side of the robot from
+ // the rear perspective in radians.
+ IntakeElasticSensors left_intake;
+
+ // Values of the series elastic encoders on the right side of the robot from
+ // the rear perspective in radians.
+ IntakeElasticSensors right_intake;
+
ArmPosition arm;
// Value of the beam breaker sensor. This value is true if the beam is
@@ -175,7 +163,11 @@
};
message Output {
- IntakeOutput intake;
+ // Voltage sent to the parts on the left side of the intake.
+ IntakeVoltage left_intake;
+
+ // Voltage sent to the parts on the right side of the intake.
+ IntakeVoltage right_intake;
// Voltage sent to the motors on the proximal joint of the arm.
double voltage_proximal;
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 601cec3..e0df3f6 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -216,10 +216,9 @@
::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
superstructure_queue_.position.MakeMessage();
- left_intake_.GetSensorValues(&position->intake.left);
- right_intake_.GetSensorValues(&position->intake.right);
+ left_intake_.GetSensorValues(&position->left_intake);
+ right_intake_.GetSensorValues(&position->right_intake);
arm_.GetSensorValues(&position->arm);
-
LOG_STRUCT(INFO, "sim position", *position);
position.Send();
}
@@ -243,8 +242,8 @@
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
- left_intake_.Simulate(superstructure_queue_.output->intake.left);
- right_intake_.Simulate(superstructure_queue_.output->intake.right);
+ left_intake_.Simulate(superstructure_queue_.output->left_intake);
+ right_intake_.Simulate(superstructure_queue_.output->right_intake);
arm_.Simulate((::Eigen::Matrix<double, 2, 1>()
<< superstructure_queue_.output->voltage_proximal,
superstructure_queue_.output->voltage_distal)
@@ -333,8 +332,8 @@
superstructure_.intake_left().state());
EXPECT_EQ(intake::IntakeSide::State::RUNNING,
superstructure_.intake_right().state());
- EXPECT_EQ(superstructure_queue_.output->intake.left.voltage_elastic, 0.0);
- EXPECT_EQ(superstructure_queue_.output->intake.right.voltage_elastic, 0.0);
+ EXPECT_EQ(superstructure_queue_.output->left_intake.voltage_elastic, 0.0);
+ EXPECT_EQ(superstructure_queue_.output->right_intake.voltage_elastic, 0.0);
}
// Tests that the intake loop can reach a goal.