Add cube scoring positions and front/back mode
Change-Id: Ifbe5785e2a502e0af2456dc4971c57fc069ba2db
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 9cfdc13..cde885c 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -34,7 +34,7 @@
))
points['GroundPickupBackCube'] = to_theta_with_circular_index_and_roll(
- -1.102, 0.224, -np.pi / 2.0, circular_index=1)
+ -1.102, 0.25, -np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -46,6 +46,19 @@
control_alpha_rolls=[(0.7, 0.0), (.9, -np.pi / 2.0)],
))
+points['GroundPickupFrontCube'] = to_theta_with_circular_index_and_roll(
+ 0.325603, 0.255189, np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToGroundPickupFrontCube",
+ start=points['Neutral'],
+ control1=np.array([3.338852196583635, 0.34968650009090885]),
+ control2=np.array([4.28246270189025, 1.492916470137478]),
+ end=points['GroundPickupFrontCube'],
+ control_alpha_rolls=[(0.4, 0.0), (.9, np.pi / 2.0)],
+ ))
+
points['ScoreBackMidConeUpPos'] = to_theta_with_circular_index_and_roll(
-1.41871454, 1.07476162, np.pi / 2.0, circular_index=0)
@@ -111,6 +124,87 @@
control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
))
+points['ScoreFrontLowCube'] = to_theta_with_circular_index_and_roll(
+ 0.325603, 0.30, np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontLowCube",
+ start=points['Neutral'],
+ control1=np.array([3.338852196583635, 0.34968650009090885]),
+ control2=np.array([4.28246270189025, 1.492916470137478]),
+ end=points['ScoreFrontLowCube'],
+ control_alpha_rolls=[(0.4, 0.0), (.9, np.pi / 2.0)],
+ ))
+
+points['ScoreBackLowCube'] = to_theta_with_circular_index_and_roll(
+ -1.102, 0.30, -np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreLowBackCube",
+ start=points['Neutral'],
+ control1=np.array([3.153228, -0.497009]),
+ control2=np.array([2.972776, -1.026820]),
+ end=points['ScoreBackLowCube'],
+ control_alpha_rolls=[(0.7, 0.0), (.9, -np.pi / 2.0)],
+ ))
+
+points['ScoreFrontMidCube'] = to_theta_with_circular_index_and_roll(
+ 0.517846, 0.87, np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontMidCube",
+ start=points["Neutral"],
+ control1=np.array([3.1310824883477952, 0.23591705727105095]),
+ control2=np.array([3.0320025094685965, 0.43674789928668933]),
+ end=points["ScoreFrontMidCube"],
+ control_alpha_rolls=[(0.4, np.pi * 0.0), (0.95, np.pi * 0.5)],
+ ))
+
+points['ScoreFrontHighCube'] = to_theta_with_circular_index_and_roll(
+ 0.901437, 1.16, np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontHighCube",
+ start=points["Neutral"],
+ control1=np.array([2.537484161662287, 0.059700523547219]),
+ control2=np.array([2.449391812539668, 0.4141564369176016]),
+ end=points["ScoreFrontHighCube"],
+ control_alpha_rolls=[(0.4, np.pi * 0.0), (0.95, np.pi * 0.5)],
+ ))
+
+points['ScoreBackMidCube'] = to_theta_with_circular_index_and_roll(
+ -1.27896, 0.84, -np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreBackMidCube",
+ start=points["Neutral"],
+ control1=np.array([3.3485646154655404, -0.4369603013926491]),
+ control2=np.array([3.2653593368256995, -0.789587049476034]),
+ end=points["ScoreBackMidCube"],
+ control_alpha_rolls=[(0.3, -np.pi * 0.0), (0.95, -np.pi * 0.5)],
+ ))
+
+# TODO(austin): This doesn't produce the next line...
+#points['ScoreBackHighCube'] = to_theta_with_circular_index_and_roll(
+# -1.60932, 1.16839, np.pi / 2.0, circular_index=0)
+points['ScoreBackHighCube'] = np.array(
+ (4.77284735761704, -1.19952193130714, -np.pi / 2.0))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreBackHighCube",
+ start=points["Neutral"],
+ control1=np.array([3.6804854484103684, -0.3494541095053125]),
+ control2=np.array([3.9889380578509517, -0.6637934755748516]),
+ end=points["ScoreBackHighCube"],
+ control_alpha_rolls=[(0.3, -np.pi * 0.0), (0.95, -np.pi * 0.5)],
+ ))
+
points['ConeDownPos'] = to_theta_with_circular_index_and_roll(0.7,
0.11,
np.pi / 2.0,
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index 5033ccf..ac926aa 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -180,11 +180,11 @@
# The limit for the proximal and distal is relative,
# so define constraints for this delta.
UPPER_DELTA_LIMIT = 0.0
-LOWER_DELTA_LIMIT = -1.9 * np.pi
+LOWER_DELTA_LIMIT = -1.98 * np.pi
# TODO(milind): put actual proximal limits
-UPPER_PROXIMAL_LIMIT = np.pi * 1.5
-LOWER_PROXIMAL_LIMIT = -np.pi
+UPPER_PROXIMAL_LIMIT = np.pi * 2.0
+LOWER_PROXIMAL_LIMIT = -np.pi * 2.0
UPPER_DISTAL_LIMIT = 0.75 * np.pi
LOWER_DISTAL_LIMIT = -0.75 * np.pi
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index c7262bf..287f0e7 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -35,9 +35,6 @@
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;
@@ -91,7 +88,11 @@
break;
case EndEffectorState::SPITTING:
// If spit requested, spit
- *roller_voltage = voltage_sign * kRollerSpitVoltage();
+ if (game_piece_ == GamePiece::CUBE) {
+ *roller_voltage = kRollerCubeSpitVoltage();
+ } else {
+ *roller_voltage = kRollerConeSpitVoltage();
+ }
if (beambreak_) {
if (!beambreak_status) {
timer_ = timestamp;
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
index d93d868..14245c8 100644
--- a/y2023/control_loops/superstructure/end_effector.h
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -15,8 +15,10 @@
class EndEffector {
public:
static constexpr double kRollerConeSuckVoltage() { return 12.0; }
+ static constexpr double kRollerConeSpitVoltage() { return -9.0; }
+
static constexpr double kRollerCubeSuckVoltage() { return -5.0; }
- static constexpr double kRollerSpitVoltage() { return -9.0; }
+ static constexpr double kRollerCubeSpitVoltage() { return 3.0; }
EndEffector();
void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 801fc55..220f3d8 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -572,7 +572,9 @@
SetEnabled(true);
WaitUntilZeroed();
- double voltage_sign = (GetParam() == GamePiece::CUBE ? -1.0 : 1.0);
+ double spit_voltage =
+ (GetParam() == GamePiece::CUBE ? EndEffector::kRollerCubeSpitVoltage()
+ : EndEffector::kRollerConeSpitVoltage());
double suck_voltage =
(GetParam() == GamePiece::CUBE ? EndEffector::kRollerCubeSuckVoltage()
: EndEffector::kRollerConeSuckVoltage());
@@ -712,8 +714,7 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
- voltage_sign * EndEffector::kRollerSpitVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), spit_voltage);
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::SPITTING);
EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GetParam());
@@ -725,8 +726,7 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(),
- voltage_sign * EndEffector::kRollerSpitVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), spit_voltage);
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::SPITTING);
EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GetParam());
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 193d3b7..489934b 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -37,20 +37,26 @@
namespace joysticks {
// TODO(milind): add correct locations
-const ButtonLocation kIntake(4, 5);
const ButtonLocation kScore(4, 4);
const ButtonLocation kSpit(4, 13);
-const ButtonLocation kMidBackTipConeScoreLeft(4, 15);
-const ButtonLocation kHighBackTipConeScoreLeft(4, 14);
-const ButtonLocation kMidBackTipConeScoreRight(3, 2);
+const ButtonLocation kHighConeScoreLeft(4, 14);
+const ButtonLocation kHighConeScoreRight(3, 1);
+
+const ButtonLocation kMidConeScoreLeft(4, 15);
+const ButtonLocation kMidConeScoreRight(3, 2);
+
+const ButtonLocation kHighCube(4, 1);
+const ButtonLocation kMidCube(4, 2);
+const ButtonLocation kLowCube(4, 3);
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);
+const ButtonLocation kSuck(4, 11);
+const ButtonLocation kBack(4, 12);
const ButtonLocation kWrist(4, 10);
@@ -63,12 +69,18 @@
CUBE = 2,
};
+enum class Side {
+ FRONT = 0,
+ BACK = 1,
+};
+
struct ArmSetpoint {
uint32_t index;
double wrist_goal;
std::optional<double> score_wrist_goal = std::nullopt;
GamePiece game_piece;
- ButtonLocation button;
+ std::vector<ButtonLocation> buttons;
+ Side side;
};
const std::vector<ArmSetpoint> setpoints = {
@@ -76,50 +88,109 @@
.index = arm::GroundPickupBackConeUpIndex(),
.wrist_goal = 0.0,
.game_piece = GamePiece::CONE_UP,
- .button = kGroundPickupConeUp,
+ .buttons = {kGroundPickupConeUp},
+ .side = Side::BACK,
},
{
.index = arm::GroundPickupBackConeDownIndex(),
.wrist_goal = 0.0,
.game_piece = GamePiece::CONE_DOWN,
- .button = kGroundPickupConeDown,
+ .buttons = {kGroundPickupConeDown},
+ .side = Side::BACK,
},
{
.index = arm::ScoreBackMidConeUpPosIndex(),
.wrist_goal = 0.55,
.game_piece = GamePiece::CONE_UP,
- .button = kMidBackTipConeScoreRight,
+ .buttons = {kMidConeScoreRight},
+ .side = Side::BACK,
},
{
.index = arm::ScoreBackMidConeDownPosIndex(),
.wrist_goal = 2.2,
.score_wrist_goal = 0.0,
.game_piece = GamePiece::CONE_DOWN,
- .button = kMidBackTipConeScoreRight,
+ .buttons = {kMidConeScoreRight},
+ .side = Side::BACK,
},
{
.index = arm::HPPickupBackConeUpIndex(),
.wrist_goal = 0.2,
.game_piece = GamePiece::CONE_UP,
- .button = kHPConePickup,
+ .buttons = {kHPConePickup},
+ .side = Side::BACK,
},
{
.index = arm::ScoreFrontHighConeUpPosIndex(),
.wrist_goal = 0.05,
.game_piece = GamePiece::CONE_UP,
- .button = kHighBackTipConeScoreLeft,
+ .buttons = {kHighConeScoreLeft, kHighConeScoreRight},
+ .side = Side::FRONT,
},
{
.index = arm::ScoreFrontMidConeUpPosIndex(),
.wrist_goal = 0.05,
.game_piece = GamePiece::CONE_UP,
- .button = kMidBackTipConeScoreLeft,
+ .buttons = {kMidConeScoreLeft, kMidConeScoreRight},
+ .side = Side::FRONT,
},
{
.index = arm::GroundPickupBackCubeIndex(),
.wrist_goal = 0.6,
.game_piece = GamePiece::CUBE,
- .button = kGroundPickupCube,
+ .buttons = {kGroundPickupCube},
+ .side = Side::BACK,
+ },
+ {
+ .index = arm::ScoreFrontMidCubeIndex(),
+ .wrist_goal = 0.6,
+ .game_piece = GamePiece::CUBE,
+ .buttons = {kMidCube},
+ .side = Side::FRONT,
+ },
+ {
+ .index = arm::ScoreBackMidCubeIndex(),
+ .wrist_goal = 0.6,
+ .score_wrist_goal = 0.0,
+ .game_piece = GamePiece::CUBE,
+ .buttons = {kMidCube},
+ .side = Side::BACK,
+ },
+ {
+ .index = arm::ScoreFrontLowCubeIndex(),
+ .wrist_goal = 0.6,
+ .game_piece = GamePiece::CUBE,
+ .buttons = {kLowCube},
+ .side = Side::FRONT,
+ },
+ {
+ .index = arm::ScoreBackLowCubeIndex(),
+ .wrist_goal = 0.6,
+ .game_piece = GamePiece::CUBE,
+ .buttons = {kLowCube},
+ .side = Side::BACK,
+ },
+ {
+ .index = arm::ScoreFrontHighCubeIndex(),
+ .wrist_goal = 0.6,
+ .game_piece = GamePiece::CUBE,
+ .buttons = {kHighCube},
+ .side = Side::FRONT,
+ },
+ {
+ .index = arm::ScoreBackHighCubeIndex(),
+ .wrist_goal = 0.6,
+ .score_wrist_goal = 0.0,
+ .game_piece = GamePiece::CUBE,
+ .buttons = {kHighCube},
+ .side = Side::BACK,
+ },
+ {
+ .index = arm::GroundPickupFrontCubeIndex(),
+ .wrist_goal = 0.6,
+ .game_piece = GamePiece::CUBE,
+ .buttons = {kGroundPickupCube},
+ .side = Side::FRONT,
},
};
@@ -173,14 +244,19 @@
wrist_goal = 0.6;
}
+ const Side current_side = data.IsPressed(kBack) ? Side::BACK : Side::FRONT;
+
// Search for the active setpoint.
for (const ArmSetpoint &setpoint : setpoints) {
- if (data.IsPressed(setpoint.button)) {
- if (setpoint.game_piece == current_game_piece_) {
- wrist_goal = setpoint.wrist_goal;
- arm_goal_position_ = setpoint.index;
- score_wrist_goal = setpoint.score_wrist_goal;
- break;
+ for (const ButtonLocation &button : setpoint.buttons) {
+ if (data.IsPressed(button)) {
+ if (setpoint.game_piece == current_game_piece_ &&
+ setpoint.side == current_side) {
+ wrist_goal = setpoint.wrist_goal;
+ arm_goal_position_ = setpoint.index;
+ score_wrist_goal = setpoint.score_wrist_goal;
+ break;
+ }
}
}
}