Don't use time of flight sensors when placing cubes
Change-Id: Ibdca3d41c7353c9b9a25d65b8991fcfbf9e7bba7
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 41d6542..521fc09 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -161,6 +161,7 @@
"//frc971/shooter_interpolation:interpolation",
"//y2023/constants:constants_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
+ "//y2023/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index 26f378c..aacfbbf 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -15,6 +15,7 @@
: joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
hint_fetcher_(event_loop->MakeFetcher<TargetSelectorHint>("/drivetrain")),
+ superstructure_status_fetcher_(event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
status_sender_(
event_loop->MakeSender<TargetSelectorStatus>("/drivetrain")),
constants_fetcher_(event_loop) {
@@ -24,6 +25,10 @@
event_loop->MakeWatcher(
"/superstructure",
[this](const y2023::control_loops::superstructure::Position &msg) {
+ // Technically this means that even if we have a cube we are relying on
+ // getting a Position message before updating the game_piece_position_
+ // to zero. But if we aren't getting position messages, then things are
+ // very broken.
game_piece_position_ =
LateralOffsetForTimeOfFlight(msg.cone_position());
});
@@ -171,7 +176,20 @@
// TODO: Maybe this already handles field side correctly? Unsure if the line
// follower ends up having positive as being robot frame relative or robot
// direction relative...
-double TargetSelector::LateralOffsetForTimeOfFlight(double reading) const {
+double TargetSelector::LateralOffsetForTimeOfFlight(double reading) {
+ superstructure_status_fetcher_.Fetch();
+ if (superstructure_status_fetcher_.get() != nullptr) {
+ switch (superstructure_status_fetcher_->game_piece()) {
+ case superstructure::GamePiece::NONE:
+ case superstructure::GamePiece::CUBE:
+ return 0.0;
+ case superstructure::GamePiece::CONE:
+ // execute logic below.
+ break;
+ }
+ } else {
+ return 0.0;
+ }
const TimeOfFlight *calibration =
CHECK_NOTNULL(constants_fetcher_.constants().robot()->tof());
// TODO(james): Use a generic interpolation table class.
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
index cab4816..bed56ce 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -7,6 +7,7 @@
#include "y2023/constants/constants_generated.h"
#include "y2023/control_loops/drivetrain/target_selector_hint_generated.h"
#include "y2023/control_loops/drivetrain/target_selector_status_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
namespace y2023::control_loops::drivetrain {
// This target selector provides the logic to choose which position to try to
@@ -47,10 +48,11 @@
private:
void UpdateAlliance();
// Returns the Y coordinate of a game piece given the time-of-flight reading.
- double LateralOffsetForTimeOfFlight(double reading) const;
+ double LateralOffsetForTimeOfFlight(double reading);
std::optional<Pose> target_pose_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<TargetSelectorHint> hint_fetcher_;
+ aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
aos::Sender<TargetSelectorStatus> status_sender_;
std::optional<TargetSelectorHintT> last_hint_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;