Fix beambreak interpretation
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ibd0d84ebe5762352cf5a6aabb74bd77c8e143cd8
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 521fc09..063e972 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -162,6 +162,7 @@
"//y2023/constants:constants_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
+ "//y2023/vision:game_pieces_fbs",
],
)
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index aacfbbf..16a0090 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -3,6 +3,7 @@
#include "aos/containers/sized_array.h"
#include "frc971/shooter_interpolation/interpolation.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/vision/game_pieces_generated.h"
namespace y2023::control_loops::drivetrain {
namespace {
@@ -15,7 +16,8 @@
: 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")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
status_sender_(
event_loop->MakeSender<TargetSelectorStatus>("/drivetrain")),
constants_fetcher_(event_loop) {
@@ -33,12 +35,14 @@
LateralOffsetForTimeOfFlight(msg.cone_position());
});
- event_loop->AddPhasedLoop([this](int){
- auto builder = status_sender_.MakeBuilder();
- auto status_builder = builder.MakeBuilder<TargetSelectorStatus>();
- status_builder.add_game_piece_position(game_piece_position_);
- builder.CheckOk(builder.Send(status_builder.Finish()));
- }, std::chrono::milliseconds(100));
+ event_loop->AddPhasedLoop(
+ [this](int) {
+ auto builder = status_sender_.MakeBuilder();
+ auto status_builder = builder.MakeBuilder<TargetSelectorStatus>();
+ status_builder.add_game_piece_position(game_piece_position_);
+ builder.CheckOk(builder.Send(status_builder.Finish()));
+ },
+ std::chrono::milliseconds(100));
}
void TargetSelector::UpdateAlliance() {
@@ -180,10 +184,13 @@
superstructure_status_fetcher_.Fetch();
if (superstructure_status_fetcher_.get() != nullptr) {
switch (superstructure_status_fetcher_->game_piece()) {
- case superstructure::GamePiece::NONE:
- case superstructure::GamePiece::CUBE:
+ case vision::Class::NONE:
+ case vision::Class::CUBE:
return 0.0;
- case superstructure::GamePiece::CONE:
+ case vision::Class::CONE_UP:
+ // execute logic below.
+ break;
+ case vision::Class::CONE_DOWN:
// execute logic below.
break;
}
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index a4cb337..2700bbc 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -33,6 +33,7 @@
deps = [
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2023/vision:game_pieces_fbs",
],
)
@@ -44,6 +45,7 @@
deps = [
"//frc971/control_loops:control_loops_ts_fbs",
"//frc971/control_loops:profiled_subsystem_ts_fbs",
+ "//y2023/vision:game_pieces_ts_fbs",
],
)
@@ -76,6 +78,7 @@
"//aos/time",
"//frc971/control_loops:control_loop",
"//y2023:constants",
+ "//y2023/vision:game_pieces_fbs",
],
)
@@ -145,6 +148,7 @@
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2023/control_loops/superstructure/roll:roll_plants",
+ "//y2023/vision:game_pieces_fbs",
],
)
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index 44b2aac..39e549b 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -22,6 +22,7 @@
"//y2023/control_loops/superstructure/arm:arm_constants",
"//y2023/control_loops/superstructure/arm:trajectory",
"//y2023/control_loops/superstructure/roll:roll_plants",
+ "//y2023/vision:game_pieces_fbs",
],
)
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 287f0e7..4d5d43e 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/control_loop.h"
+#include "y2023/vision/game_pieces_generated.h"
namespace y2023 {
namespace control_loops {
@@ -12,7 +13,7 @@
EndEffector::EndEffector()
: state_(EndEffectorState::IDLE),
- game_piece_(GamePiece::NONE),
+ game_piece_(vision::Class::NONE),
timer_(aos::monotonic_clock::min_time),
beambreak_(false) {}
@@ -25,16 +26,22 @@
constexpr double kMinCurrent = 40.0;
constexpr double kMaxConePosition = 0.92;
- bool beambreak_status = (beambreak || (falcon_current > kMinCurrent &&
- cone_position < kMaxConePosition));
-
// Let them switch game pieces
- if (roller_goal == RollerGoal::INTAKE_CONE) {
- game_piece_ = GamePiece::CONE;
+ if (roller_goal == RollerGoal::INTAKE_CONE_UP) {
+ game_piece_ = vision::Class::CONE_UP;
+ } else if (roller_goal == RollerGoal::INTAKE_CONE_DOWN) {
+ game_piece_ = vision::Class::CONE_DOWN;
} else if (roller_goal == RollerGoal::INTAKE_CUBE) {
- game_piece_ = GamePiece::CUBE;
+ game_piece_ = vision::Class::CUBE;
}
+ bool beambreak_status =
+ (((game_piece_ == vision::Class::CUBE ||
+ game_piece_ == vision::Class::CONE_UP) &&
+ beambreak) ||
+ ((game_piece_ == vision::Class::CONE_DOWN &&
+ falcon_current > kMinCurrent && cone_position < kMaxConePosition)));
+
// Go into spitting if we were told to, no matter where we are
if (roller_goal == RollerGoal::SPIT && state_ != EndEffectorState::SPITTING) {
state_ = EndEffectorState::SPITTING;
@@ -47,7 +54,8 @@
switch (state_) {
case EndEffectorState::IDLE:
// If idle and intake requested, intake
- if (roller_goal == RollerGoal::INTAKE_CONE ||
+ if (roller_goal == RollerGoal::INTAKE_CONE_UP ||
+ roller_goal == RollerGoal::INTAKE_CONE_DOWN ||
roller_goal == RollerGoal::INTAKE_CUBE ||
roller_goal == RollerGoal::INTAKE_LAST) {
state_ = EndEffectorState::INTAKING;
@@ -56,7 +64,8 @@
break;
case EndEffectorState::INTAKING:
// If intaking and beam break is not triggered, keep intaking
- if (roller_goal == RollerGoal::INTAKE_CONE ||
+ if (roller_goal == RollerGoal::INTAKE_CONE_UP ||
+ roller_goal == RollerGoal::INTAKE_CONE_DOWN ||
roller_goal == RollerGoal::INTAKE_CUBE ||
roller_goal == RollerGoal::INTAKE_LAST) {
timer_ = timestamp;
@@ -72,7 +81,7 @@
break;
}
- if (game_piece_ == GamePiece::CUBE) {
+ if (game_piece_ == vision::Class::CUBE) {
*roller_voltage = kRollerCubeSuckVoltage();
} else {
*roller_voltage = kRollerConeSuckVoltage();
@@ -88,7 +97,7 @@
break;
case EndEffectorState::SPITTING:
// If spit requested, spit
- if (game_piece_ == GamePiece::CUBE) {
+ if (game_piece_ == vision::Class::CUBE) {
*roller_voltage = kRollerCubeSpitVoltage();
} else {
*roller_voltage = kRollerConeSpitVoltage();
@@ -100,7 +109,7 @@
} else if (timestamp > timer_ + constants::Values::kExtraSpittingTime()) {
// Finished spitting
state_ = EndEffectorState::IDLE;
- game_piece_ = GamePiece::NONE;
+ game_piece_ = vision::Class::NONE;
}
break;
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
index 14245c8..5ae96da 100644
--- a/y2023/control_loops/superstructure/end_effector.h
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -7,6 +7,7 @@
#include "y2023/constants.h"
#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2023/vision/game_pieces_generated.h"
namespace y2023 {
namespace control_loops {
@@ -26,12 +27,12 @@
double cone_position, bool beambreak,
double *intake_roller_voltage);
EndEffectorState state() const { return state_; }
- GamePiece game_piece() const { return game_piece_; }
+ vision::Class game_piece() const { return game_piece_; }
void Reset();
private:
EndEffectorState state_;
- GamePiece game_piece_;
+ vision::Class game_piece_;
aos::monotonic_clock::time_point timer_;
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index ee99f1c..670351a 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -4,10 +4,11 @@
enum RollerGoal: ubyte {
IDLE = 0,
- INTAKE_CONE = 1,
+ INTAKE_CONE_UP = 1,
INTAKE_CUBE = 2,
INTAKE_LAST = 3,
SPIT = 4,
+ INTAKE_CONE_DOWN = 5,
}
table Goal {
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index ee440ed..e2524c1 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -562,10 +562,10 @@
class SuperstructureBeambreakTest
: public SuperstructureTest,
- public ::testing::WithParamInterface<GamePiece> {
+ public ::testing::WithParamInterface<vision::Class> {
public:
- void SetBeambreak(GamePiece game_piece, bool status) {
- if (game_piece == GamePiece::CONE) {
+ void SetBeambreak(vision::Class game_piece, bool status) {
+ if (game_piece == vision::Class::CONE_UP) {
// TODO(milind): handle cone
} else {
superstructure_plant_.set_end_effector_cube_beam_break(status);
@@ -577,12 +577,20 @@
SetEnabled(true);
WaitUntilZeroed();
- double spit_voltage =
- (GetParam() == GamePiece::CUBE ? EndEffector::kRollerCubeSpitVoltage()
- : EndEffector::kRollerConeSpitVoltage());
- double suck_voltage =
- (GetParam() == GamePiece::CUBE ? EndEffector::kRollerCubeSuckVoltage()
- : EndEffector::kRollerConeSuckVoltage());
+ double spit_voltage = (GetParam() == vision::Class::CUBE
+ ? EndEffector::kRollerCubeSpitVoltage()
+ : EndEffector::kRollerConeSpitVoltage());
+ double suck_voltage = (GetParam() == vision::Class::CUBE
+ ? EndEffector::kRollerCubeSuckVoltage()
+ : EndEffector::kRollerConeSuckVoltage());
+
+ RollerGoal roller_goal = RollerGoal::INTAKE_CUBE;
+
+ if (GetParam() == vision::Class::CONE_DOWN) {
+ roller_goal = RollerGoal::INTAKE_CONE_DOWN;
+ } else if (GetParam() == vision::Class::CONE_UP) {
+ roller_goal = RollerGoal::INTAKE_CONE_UP;
+ }
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -591,9 +599,7 @@
goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
- goal_builder.add_roller_goal(GetParam() == GamePiece::CONE
- ? RollerGoal::INTAKE_CONE
- : RollerGoal::INTAKE_CUBE);
+ goal_builder.add_roller_goal(roller_goal);
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
@@ -668,9 +674,7 @@
goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
- goal_builder.add_roller_goal(GetParam() == GamePiece::CONE
- ? RollerGoal::INTAKE_CONE
- : RollerGoal::INTAKE_CUBE);
+ goal_builder.add_roller_goal(roller_goal);
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
@@ -757,7 +761,7 @@
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::IDLE);
- EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GamePiece::NONE);
+ EXPECT_EQ(superstructure_status_fetcher_->game_piece(), vision::Class::NONE);
}
// Tests that we don't freak out without a goal.
@@ -834,7 +838,7 @@
// TODO(milind): add cone
INSTANTIATE_TEST_SUITE_P(EndEffectorGoal, SuperstructureBeambreakTest,
- ::testing::Values(GamePiece::CUBE));
+ ::testing::Values(vision::Class::CUBE));
} // namespace testing
} // namespace superstructure
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index 80a0d3d..5381b0a 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -1,4 +1,5 @@
include "frc971/control_loops/control_loops.fbs";
+include "y2023/vision/game_pieces.fbs";
include "frc971/control_loops/profiled_subsystem.fbs";
namespace y2023.control_loops.superstructure;
@@ -76,12 +77,6 @@
SPITTING = 3,
}
-enum GamePiece : ubyte {
- NONE = 0,
- CONE = 1,
- CUBE = 2,
-}
-
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
@@ -94,7 +89,7 @@
wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
end_effector_state:EndEffectorState (id: 4);
- game_piece:GamePiece (id: 5);
+ game_piece:vision.Class (id: 5);
}
root_type Status;
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 43962dd..68fe6af 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -334,10 +334,10 @@
std::optional<double> score_wrist_goal = std::nullopt;
if (data.IsPressed(kGroundPickupConeUp) || data.IsPressed(kHPConePickup)) {
- roller_goal = RollerGoal::INTAKE_CONE;
+ roller_goal = RollerGoal::INTAKE_CONE_UP;
current_game_piece_ = GamePiece::CONE_UP;
} else if (data.IsPressed(kGroundPickupConeDownBase)) {
- roller_goal = RollerGoal::INTAKE_CONE;
+ roller_goal = RollerGoal::INTAKE_CONE_DOWN;
current_game_piece_ = GamePiece::CONE_DOWN;
} else if (data.IsPressed(kGroundPickupCube)) {
roller_goal = RollerGoal::INTAKE_CUBE;
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index e90b825..68ba833 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,4 +1,5 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
cc_binary(
name = "camera_reader",
@@ -239,3 +240,10 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
)
+
+flatbuffer_ts_library(
+ name = "game_pieces_ts_fbs",
+ srcs = ["game_pieces.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023/vision/game_pieces.fbs b/y2023/vision/game_pieces.fbs
index e981712..aef52e4 100644
--- a/y2023/vision/game_pieces.fbs
+++ b/y2023/vision/game_pieces.fbs
@@ -2,9 +2,10 @@
// Object class.
enum Class : byte {
- CONE_DOWN,
- CONE_UP,
- CUBE
+ NONE = 0,
+ CONE_UP = 1,
+ CUBE = 2,
+ CONE_DOWN = 3,
}
// Bounding box dimensions and position.
@@ -26,4 +27,4 @@
game_pieces:[GamePiece] (id: 0);
}
-root_type GamePieces;
\ No newline at end of file
+root_type GamePieces;