Send 0.95 from TOF controller if there is no cone.
This lets us detect cone presence separately from unplugged sensor.
Change-Id: I0f70c1229bd88b5c50ed2fdf043361d072c41402
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 9d5d564..617464b 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -79,7 +79,7 @@
EndEffectorState end_effector_state = end_effector_.RunIteration(
timestamp,
unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
- position->end_effector_cone_beam_break(),
+ false,
position->end_effector_cube_beam_break(), &output_struct.roller_voltage);
if (output) {
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index b94054d..f9b8548 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -244,18 +244,14 @@
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_arm(arm_offset);
position_builder.add_wrist(wrist_offset);
- position_builder.add_end_effector_cone_beam_break(
- end_effector_cone_beam_break_);
position_builder.add_end_effector_cube_beam_break(
end_effector_cube_beam_break_);
+ // TODO(milind): put into our state
+ position_builder.add_cone_position(0.95);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
- void set_end_effector_cone_beam_break(bool triggered) {
- end_effector_cone_beam_break_ = triggered;
- }
-
void set_end_effector_cube_beam_break(bool triggered) {
end_effector_cube_beam_break_ = triggered;
}
@@ -268,7 +264,6 @@
ArmSimulation arm_;
AbsoluteEncoderSimulator wrist_;
- bool end_effector_cone_beam_break_;
bool end_effector_cube_beam_break_;
::aos::Sender<Position> superstructure_position_sender_;
@@ -569,7 +564,7 @@
public:
void SetBeambreak(GamePiece game_piece, bool status) {
if (game_piece == GamePiece::kCone) {
- superstructure_plant_.set_end_effector_cone_beam_break(status);
+ // TODO(milind): handle cone
} else {
superstructure_plant_.set_end_effector_cube_beam_break(status);
}
@@ -807,8 +802,9 @@
VerifyNearGoal();
}
+// TODO(milind): add cone
INSTANTIATE_TEST_SUITE_P(EndEffectorGoal, SuperstructureBeambreakTest,
- ::testing::Values(GamePiece::kCone, GamePiece::kCube));
+ ::testing::Values(GamePiece::kCube));
} // namespace testing
} // namespace superstructure
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index 927fe10..cf8a843 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -26,7 +26,7 @@
wrist:frc971.AbsolutePosition (id: 1);
// If this is true, the cone beam break is triggered.
- end_effector_cone_beam_break:bool (id: 2);
+ cone_position:double (id: 2);
// If this is true, the cube beam break is triggered.
end_effector_cube_beam_break:bool (id: 3);