Fix beambreak interpretation
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ibd0d84ebe5762352cf5a6aabb74bd77c8e143cd8
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;