Add code to intake a cube
Voltage is flipped for cubes.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I16b07e4c6b21a173c10fa682f39103aff8aa9379
diff --git a/y2023/constants.h b/y2023/constants.h
index 9cb851b..059a3f0 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -143,7 +143,6 @@
// Rollers
static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
static constexpr double kRollerStatorCurrentLimit() { return 100.0; }
- static constexpr double kRollerVoltage() { return 12.0; }
// Game object is fed into end effector for at least this time
static constexpr std::chrono::milliseconds kExtraIntakingTime() {
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index f07ddff..874cfad 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -11,9 +11,12 @@
using ::aos::monotonic_clock;
EndEffector::EndEffector()
- : state_(EndEffectorState::IDLE), beambreak_(false) {}
+ : state_(EndEffectorState::IDLE),
+ game_piece_(GamePiece::NONE),
+ timer_(aos::monotonic_clock::min_time),
+ beambreak_(false) {}
-EndEffectorState EndEffector::RunIteration(
+void EndEffector::RunIteration(
const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
double falcon_current, double cone_position, bool beambreak,
double *roller_voltage) {
@@ -25,6 +28,16 @@
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;
+ } else if (roller_goal == RollerGoal::INTAKE_CUBE) {
+ game_piece_ = GamePiece::CUBE;
+ }
+
+ // Cube voltage is flipped
+ double voltage_sign = (game_piece_ == GamePiece::CUBE ? -1.0 : 1.0);
+
// Go into spitting if we were told to, no matter where we are
if (roller_goal == RollerGoal::SPIT && state_ != EndEffectorState::SPITTING) {
state_ = EndEffectorState::SPITTING;
@@ -37,7 +50,9 @@
switch (state_) {
case EndEffectorState::IDLE:
// If idle and intake requested, intake
- if (roller_goal == RollerGoal::INTAKE) {
+ if (roller_goal == RollerGoal::INTAKE_CONE ||
+ roller_goal == RollerGoal::INTAKE_CUBE ||
+ roller_goal == RollerGoal::INTAKE_LAST) {
state_ = EndEffectorState::INTAKING;
timer_ = timestamp;
}
@@ -54,7 +69,11 @@
break;
}
- *roller_voltage = constants::Values::kRollerVoltage();
+ if (game_piece_ == GamePiece::CUBE) {
+ *roller_voltage = kRollerCubeSuckVoltage();
+ } else {
+ *roller_voltage = kRollerConeSuckVoltage();
+ }
break;
case EndEffectorState::LOADED:
@@ -66,7 +85,7 @@
break;
case EndEffectorState::SPITTING:
// If spit requested, spit
- *roller_voltage = -constants::Values::kRollerVoltage();
+ *roller_voltage = voltage_sign * kRollerSpitVoltage();
if (beambreak_) {
if (!beambreak_status) {
timer_ = timestamp;
@@ -74,14 +93,13 @@
} else if (timestamp > timer_ + constants::Values::kExtraSpittingTime()) {
// Finished spitting
state_ = EndEffectorState::IDLE;
+ game_piece_ = GamePiece::NONE;
}
break;
}
beambreak_ = beambreak_status;
-
- return state_;
}
void EndEffector::Reset() { state_ = EndEffectorState::IDLE; }
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
index b174e16..d93d868 100644
--- a/y2023/control_loops/superstructure/end_effector.h
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -14,17 +14,24 @@
class EndEffector {
public:
+ static constexpr double kRollerConeSuckVoltage() { return 12.0; }
+ static constexpr double kRollerCubeSuckVoltage() { return -5.0; }
+ static constexpr double kRollerSpitVoltage() { return -9.0; }
+
EndEffector();
- EndEffectorState RunIteration(
- const ::aos::monotonic_clock::time_point timestamp,
- RollerGoal roller_goal, double falcon_current, double cone_position,
- bool beambreak, double *intake_roller_voltage);
+ void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
+ RollerGoal roller_goal, double falcon_current,
+ double cone_position, bool beambreak,
+ double *intake_roller_voltage);
EndEffectorState state() const { return state_; }
+ GamePiece game_piece() const { return game_piece_; }
void Reset();
private:
- EndEffectorState state_ = EndEffectorState::IDLE;
- aos::monotonic_clock::time_point timer_ = aos::monotonic_clock::min_time;
+ EndEffectorState state_;
+ GamePiece game_piece_;
+
+ aos::monotonic_clock::time_point timer_;
bool beambreak_;
};
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 3bd9488..f59626c 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -76,7 +76,7 @@
output != nullptr ? &(output_struct.wrist_voltage) : nullptr,
status->fbb());
- EndEffectorState end_effector_state = end_effector_.RunIteration(
+ end_effector_.RunIteration(
timestamp,
unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
position->has_roller_falcon()
@@ -94,7 +94,9 @@
status_builder.add_estopped(wrist_.estopped() || arm_.estopped());
status_builder.add_arm(arm_status_offset);
status_builder.add_wrist(wrist_offset);
- status_builder.add_end_effector_state(end_effector_state);
+ status_builder.add_end_effector_state(end_effector_.state());
+ // TODO(milind): integrate this with ML game piece detection somehow
+ status_builder.add_game_piece(end_effector_.game_piece());
(void)status->Send(status_builder.Finish());
}
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 8f2d5ab..ee99f1c 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -4,8 +4,10 @@
enum RollerGoal: ubyte {
IDLE = 0,
- INTAKE = 1,
- SPIT = 2,
+ INTAKE_CONE = 1,
+ INTAKE_CUBE = 2,
+ INTAKE_LAST = 3,
+ SPIT = 4,
}
table Goal {
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index e2b8a34..1615a29 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -53,11 +53,6 @@
Superstructure::AbsoluteEncoderSubsystem::State,
constants::Values::AbsEncoderConstants>;
-enum GamePiece {
- kCone,
- kCube,
-};
-
class ArmSimulation {
public:
explicit ArmSimulation(
@@ -565,7 +560,7 @@
public ::testing::WithParamInterface<GamePiece> {
public:
void SetBeambreak(GamePiece game_piece, bool status) {
- if (game_piece == GamePiece::kCone) {
+ if (game_piece == GamePiece::CONE) {
// TODO(milind): handle cone
} else {
superstructure_plant_.set_end_effector_cube_beam_break(status);
@@ -577,6 +572,11 @@
SetEnabled(true);
WaitUntilZeroed();
+ double voltage_sign = (GetParam() == GamePiece::CUBE ? -1.0 : 1.0);
+ double suck_voltage =
+ (GetParam() == GamePiece::CUBE ? EndEffector::kRollerCubeSuckVoltage()
+ : EndEffector::kRollerConeSuckVoltage());
+
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -584,7 +584,9 @@
goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
goal_builder.add_trajectory_override(false);
- goal_builder.add_roller_goal(RollerGoal::INTAKE);
+ goal_builder.add_roller_goal(GetParam() == GamePiece::CONE
+ ? RollerGoal::INTAKE_CONE
+ : RollerGoal::INTAKE_CUBE);
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
@@ -596,10 +598,10 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
- constants::Values::kRollerVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::INTAKING);
+ EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GetParam());
SetBeambreak(GetParam(), true);
@@ -613,6 +615,7 @@
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::LOADED);
+ EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GetParam());
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -634,10 +637,10 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
- constants::Values::kRollerVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::INTAKING);
+ EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GetParam());
// Checking that we go back to idle after beambreak is lost and we
// set our goal to idle.
@@ -648,6 +651,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(), GetParam());
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -656,7 +660,9 @@
goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
goal_builder.add_trajectory_override(false);
- goal_builder.add_roller_goal(RollerGoal::INTAKE);
+ goal_builder.add_roller_goal(GetParam() == GamePiece::CONE
+ ? RollerGoal::INTAKE_CONE
+ : RollerGoal::INTAKE_CUBE);
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
@@ -668,10 +674,10 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
- constants::Values::kRollerVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::INTAKING);
+ EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GetParam());
SetBeambreak(GetParam(), true);
@@ -684,6 +690,7 @@
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::LOADED);
+ EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GetParam());
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -705,9 +712,10 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
- -constants::Values::kRollerVoltage());
+ voltage_sign * EndEffector::kRollerSpitVoltage());
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::SPITTING);
+ EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GetParam());
SetBeambreak(GetParam(), false);
@@ -717,9 +725,10 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
- -constants::Values::kRollerVoltage());
+ voltage_sign * EndEffector::kRollerSpitVoltage());
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::SPITTING);
+ EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GetParam());
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -742,6 +751,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);
}
// Tests that we don't freak out without a goal.
@@ -818,7 +828,7 @@
// TODO(milind): add cone
INSTANTIATE_TEST_SUITE_P(EndEffectorGoal, SuperstructureBeambreakTest,
- ::testing::Values(GamePiece::kCube));
+ ::testing::Values(GamePiece::CUBE));
} // namespace testing
} // namespace superstructure
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index 2966d75..80a0d3d 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -76,6 +76,12 @@
SPITTING = 3,
}
+enum GamePiece : ubyte {
+ NONE = 0,
+ CONE = 1,
+ CUBE = 2,
+}
+
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
@@ -88,6 +94,7 @@
wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
end_effector_state:EndEffectorState (id: 4);
+ game_piece:GamePiece (id: 5);
}
root_type Status;
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 58f7df1..92f79d8 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -47,6 +47,7 @@
const ButtonLocation kGroundPickupConeUp(4, 7);
const ButtonLocation kGroundPickupConeDown(4, 8);
+const ButtonLocation kGroundPickupCube(4, 10);
const ButtonLocation kHPConePickup(4, 6);
const ButtonLocation kSuck(4, 12);
@@ -152,11 +153,14 @@
std::optional<double> score_wrist_goal = std::nullopt;
if (data.IsPressed(kGroundPickupConeUp) || data.IsPressed(kHPConePickup)) {
- roller_goal = RollerGoal::INTAKE;
+ roller_goal = RollerGoal::INTAKE_CONE;
current_game_piece_ = GamePiece::CONE_UP;
} else if (data.IsPressed(kGroundPickupConeDown)) {
- roller_goal = RollerGoal::INTAKE;
+ roller_goal = RollerGoal::INTAKE_CONE;
current_game_piece_ = GamePiece::CONE_DOWN;
+ } else if (data.IsPressed(kGroundPickupCube)) {
+ roller_goal = RollerGoal::INTAKE_CUBE;
+ current_game_piece_ = GamePiece::CUBE;
}
// Search for the active setpoint.
@@ -172,7 +176,7 @@
}
if (data.IsPressed(kSuck)) {
- roller_goal = RollerGoal::INTAKE;
+ roller_goal = RollerGoal::INTAKE_LAST;
} else if (data.IsPressed(kSpit)) {
if (score_wrist_goal.has_value()) {
wrist_goal = score_wrist_goal.value();