All the setpoints and tip scoring paths
This introduces the concept of a scoring trajectory so we can score the
tip cones.
Change-Id: Id1fefe37e916bf02956250b59b704363deda7ad3
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/constants.cc b/y2023/constants.cc
index eed17c4..4bd2096 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -105,7 +105,7 @@
0.0201047336425017 - 1.0173426655158 - 0.186085272847293 - 0.0317706563397807;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 5.78862525947414;
+ 0.894159203288852;
break;
diff --git a/y2023/control_loops/drivetrain/drivetrain_base.cc b/y2023/control_loops/drivetrain/drivetrain_base.cc
index e8fdb3c..4efba61 100644
--- a/y2023/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_base.cc
@@ -75,7 +75,8 @@
10.0 / ::std::pow(12.0, 2))
.finished()
.asDiagonal()),
- .max_controllable_offset = 0.5}};
+ .max_controllable_offset = 0.5},
+ };
return kDrivetrainConfig;
};
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 12a8967..e8589ef 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -4,6 +4,18 @@
from y2023.control_loops.python.graph_tools import *
+
+def ThetaSegment(name, start, end):
+ control = np.array([(start[0] + end[0]) / 2.0, (start[1] + end[1]) / 2.0])
+ return ThetaSplineSegment(
+ name=name,
+ start=start,
+ control1=control,
+ control2=control,
+ end=end,
+ )
+
+
named_segments = []
points = {}
@@ -37,7 +49,7 @@
points[
'GroundPickupFrontConeDownBase'] = to_theta_with_circular_index_and_roll(
- 0.263207, 0.24, -np.pi / 2.0, circular_index=0)
+ 0.30, 0.24, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -75,6 +87,136 @@
control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
))
+points['ScoreBackLowConeDownTip'] = to_theta_with_circular_index_and_roll(
+ -1.17422, 0.441203, np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreBackLowConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([3.0959727041167358, -0.48933188185224896]),
+ control2=np.array([3.11854219540683, -1.0398000886366843]),
+ end=points['ScoreBackLowConeDownTip'],
+ control_alpha_rolls=[(0.20, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreFrontLowConeDownTip'] = to_theta_with_circular_index_and_roll(
+ 0.327783, 0.430704, np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontLowConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([3.6217558044411176, 0.6335548380532725]),
+ control2=np.array([4.2557660430407935, 1.0411926555706872]),
+ end=points['ScoreFrontLowConeDownTip'],
+ control_alpha_rolls=[(0.20, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreBackMidConeDownTip'] = to_theta_with_circular_index_and_roll(
+ -1.49, 0.818521, -np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreBackMidConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([3.193704394908777, -0.46076706416611657]),
+ control2=np.array([3.6421839688861786, -0.8129214904599373]),
+ end=points['ScoreBackMidConeDownTip'],
+ control_alpha_rolls=[(0.20, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+points[
+ 'ScoreBackMidConeDownTipPlaced'] = to_theta_with_circular_index_and_roll(
+ -1.43, 0.65, -np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreBackMidConeDownTipPlaced",
+ start=points['Neutral'],
+ control1=np.array([3.193704394908777, -0.46076706416611657]),
+ control2=np.array([3.6421839688861786, -0.8129214904599373]),
+ end=points['ScoreBackMidConeDownTipPlaced'],
+ control_alpha_rolls=[(0.20, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+named_segments.append(
+ ThetaSegment(
+ name="ScoreBackMidConeDownTipToScoreBackMidConeDownTipPlaced",
+ start=points['ScoreBackMidConeDownTip'],
+ end=points['ScoreBackMidConeDownTipPlaced'],
+ ))
+
+points['ScoreFrontMidConeDownTip'] = np.array(
+ (6.37001629521978, 2.04450540030891, np.pi / 2.0))
+#to_theta_with_circular_index_and_roll(
+#0.708449, 0.869738, np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontMidConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([4.579377666056791, 0.3789471836198275]),
+ control2=np.array([5.140992799899862, 1.5135884307866865]),
+ end=points['ScoreFrontMidConeDownTip'],
+ control_alpha_rolls=[(0.50, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreFrontMidConeDownTipPlaced'] = np.array(
+ (6.42001629521978, 2.30450540030891, np.pi / 2.0))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontMidConeDownTipPlaced",
+ start=points['Neutral'],
+ control1=np.array([4.579377666056791, 0.3789471836198275]),
+ control2=np.array([5.140992799899862, 1.5135884307866865]),
+ end=points['ScoreFrontMidConeDownTipPlaced'],
+ control_alpha_rolls=[(0.50, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+named_segments.append(
+ ThetaSegment(
+ name="ScoreFrontMidConeDownTipToScoreFrontMidConeDownTipPlaced",
+ start=points['ScoreFrontMidConeDownTip'],
+ end=points['ScoreFrontMidConeDownTipPlaced'],
+ ))
+
+points['ScoreFrontHighConeDownTip'] = np.array(
+ (7.07190783461154, 1.55094570328448, np.pi / 2.0))
+#to_theta_with_circular_index_and_roll(
+#0.708449, 0.869738, np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontHighConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([4.579377666056791, 0.3789471836198275]),
+ control2=np.array([5.140992799899862, 1.5135884307866865]),
+ end=points['ScoreFrontHighConeDownTip'],
+ control_alpha_rolls=[(0.50, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreFrontHighConeDownTipPlaced'] = np.array(
+ (6.93190783461154, 1.80094570328448, np.pi / 2.0))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontHighConeDownTipPlaced",
+ start=points['Neutral'],
+ control1=np.array([5.997741842590495, 1.8354263885166913]),
+ control2=np.array([6.141018843972322, 1.0777341552037734]),
+ end=points['ScoreFrontHighConeDownTipPlaced'],
+ control_alpha_rolls=[(0.50, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+named_segments.append(
+ ThetaSegment(
+ name="ScoreFrontHighConeDownTipToScoreFrontHighConeDownTipPlaced",
+ start=points['ScoreFrontHighConeDownTip'],
+ end=points['ScoreFrontHighConeDownTipPlaced'],
+ ))
+
points['ScoreFrontHighConeDownBase'] = to_theta_with_circular_index_and_roll(
1.04686, 1.13243, -np.pi / 2.0, circular_index=0)
@@ -89,7 +231,7 @@
))
points['GroundPickupBackCube'] = to_theta_with_circular_index_and_roll(
- -1.102, 0.30, -np.pi / 2.0, circular_index=1)
+ -1.102, 0.28, -np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -115,7 +257,7 @@
))
points['ScoreBackMidConeUp'] = to_theta_with_circular_index_and_roll(
- -1.33013, 1.08354, np.pi / 2.0, circular_index=1)
+ -1.45013, 1.04354, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -161,7 +303,7 @@
))
points['ScoreBackMidConeDownBase'] = to_theta_with_circular_index_and_roll(
- -1.37792406, 0.81332449, np.pi / 2.0, circular_index=1)
+ -1.37792406, 0.87332449, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -187,7 +329,7 @@
))
points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.1050539, 1.34, np.pi / 2.0, circular_index=0)
+ -1.1050539, 1.325, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -228,7 +370,7 @@
))
points['ScoreFrontMidConeUp'] = to_theta_with_circular_index_and_roll(
- 0.43740453, 1.06330555, -np.pi / 2.0, circular_index=0)
+ 0.64, 1.03, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -241,7 +383,7 @@
))
points['ScoreFrontLowCube'] = to_theta_with_circular_index_and_roll(
- 0.325603, 0.30, np.pi / 2.0, circular_index=0)
+ 0.325603, 0.39, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index ac926aa..b4d047a 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -183,7 +183,7 @@
LOWER_DELTA_LIMIT = -1.98 * np.pi
# TODO(milind): put actual proximal limits
-UPPER_PROXIMAL_LIMIT = np.pi * 2.0
+UPPER_PROXIMAL_LIMIT = np.pi * 3.0
LOWER_PROXIMAL_LIMIT = -np.pi * 2.0
UPPER_DISTAL_LIMIT = 0.75 * np.pi
@@ -193,12 +193,36 @@
LOWER_ROLL_JOINT_LIMIT = -0.75 * np.pi
-def arm_past_limit(theta1, theta2, theta3):
+def arm_past_limit(theta1, theta2, theta3, verbose=True):
delta = theta2 - theta1
- return delta > UPPER_DELTA_LIMIT or delta < LOWER_DELTA_LIMIT or \
- theta1 > UPPER_PROXIMAL_LIMIT or theta1 < LOWER_PROXIMAL_LIMIT or \
- theta2 > UPPER_DISTAL_LIMIT or theta2 < LOWER_DISTAL_LIMIT or \
- theta3 > UPPER_ROLL_JOINT_LIMIT or theta3 < LOWER_ROLL_JOINT_LIMIT
+ if delta > UPPER_DELTA_LIMIT or delta < LOWER_DELTA_LIMIT:
+ if verbose:
+ print(
+ f'Delta {delta} outside {LOWER_DELTA_LIMIT}, {UPPER_DELTA_LIMIT}'
+ )
+ return True
+ if theta1 > UPPER_PROXIMAL_LIMIT or theta1 < LOWER_PROXIMAL_LIMIT:
+ if verbose:
+ print(
+ f'Proximal {theta1} outside {LOWER_PROXIMAL_LIMIT}, {UPPER_PROXIMAL_LIMIT}'
+ )
+ return True
+
+ if theta2 > UPPER_DISTAL_LIMIT or theta2 < LOWER_DISTAL_LIMIT:
+ if verbose:
+ print(
+ f'Proximal {theta2} outside {LOWER_DISTAL_LIMIT}, {UPPER_DISTAL_LIMIT}'
+ )
+ return True
+
+ if theta3 > UPPER_ROLL_JOINT_LIMIT or theta3 < LOWER_ROLL_JOINT_LIMIT:
+ if verbose:
+ print(
+ f'Proximal {theta3} outside {LOWER_ROLL_JOINT_LIMIT}, {UPPER_ROLL_JOINT_LIMIT}'
+ )
+ return True
+
+ return False
def get_circular_index(theta):
@@ -376,7 +400,7 @@
def arm_past_limit(self, points, verbose=True):
for point in points:
- if arm_past_limit(*point):
+ if arm_past_limit(*point, verbose=verbose):
if verbose:
print("Arm past limit for path %s in point %s" %
(self.name, point))
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
index 5ae96da..2d95115 100644
--- a/y2023/control_loops/superstructure/end_effector.h
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -18,7 +18,7 @@
static constexpr double kRollerConeSuckVoltage() { return 12.0; }
static constexpr double kRollerConeSpitVoltage() { return -9.0; }
- static constexpr double kRollerCubeSuckVoltage() { return -5.0; }
+ static constexpr double kRollerCubeSuckVoltage() { return -7.0; }
static constexpr double kRollerCubeSpitVoltage() { return 3.0; }
EndEffector();
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 4c4b1e3..9efd848 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -42,6 +42,9 @@
namespace input {
namespace joysticks {
+constexpr double kConeWrist = 0.4;
+constexpr double kCubeWrist = 1.0;
+
// TODO(milind): add correct locations
const ButtonLocation kDriverSpit(2, 1);
const ButtonLocation kSpit(4, 13);
@@ -60,7 +63,7 @@
const ButtonLocation kLowCube(4, 3);
const ButtonLocation kGroundPickupConeUp(4, 7);
-const ButtonLocation kGroundPickupConeDownBase(4, 8);
+const ButtonLocation kGroundPickupConeDown(4, 8);
const ButtonLocation kGroundPickupCube(4, 10);
const ButtonLocation kHPConePickup(4, 6);
@@ -70,6 +73,9 @@
const ButtonLocation kWrist(4, 10);
const ButtonLocation kStayIn(3, 4);
+const ButtonLocation kConeDownTip(4, 4);
+const ButtonLocation kConeDownBase(4, 5);
+
namespace superstructure = y2023::control_loops::superstructure;
namespace arm = superstructure::arm;
@@ -77,6 +83,7 @@
CONE_UP = 0,
CONE_DOWN = 1,
CUBE = 2,
+ CONE_TIP = 4,
};
struct ButtonData {
@@ -86,6 +93,7 @@
struct ArmSetpoint {
uint32_t index;
+ std::optional<uint32_t> place_index = std::nullopt;
double wrist_goal;
std::optional<double> score_wrist_goal = std::nullopt;
GamePiece game_piece;
@@ -97,35 +105,88 @@
const std::vector<ArmSetpoint> setpoints = {
{
.index = arm::GroundPickupBackConeUpIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = 0.7,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kGroundPickupConeUp}},
.side = Side::BACK,
},
{
.index = arm::GroundPickupFrontConeUpIndex(),
- .wrist_goal = 0.2,
+ .wrist_goal = 0.6,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kGroundPickupConeUp}},
.side = Side::FRONT,
},
{
.index = arm::GroundPickupBackConeDownBaseIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_DOWN,
- .buttons = {{kGroundPickupConeDownBase}},
+ .buttons = {{kGroundPickupConeDown}},
.side = Side::BACK,
},
{
.index = arm::GroundPickupFrontConeDownBaseIndex(),
- .wrist_goal = 0.2,
+ .wrist_goal = 0.6,
.game_piece = GamePiece::CONE_DOWN,
- .buttons = {{kGroundPickupConeDownBase}},
+ .buttons = {{kGroundPickupConeDown}},
.side = Side::FRONT,
},
{
- .index = arm::ScoreBackMidConeUpIndex(),
+ .index = arm::ScoreBackLowConeDownTipIndex(),
+ .wrist_goal = 0.7,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kLowConeScoreRight, SpotSelectionHint::RIGHT},
+ {kLowCube, SpotSelectionHint::MIDDLE},
+ {kLowConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::BACK,
+ .row_hint = RowSelectionHint::BOTTOM,
+ },
+ {
+ .index = arm::ScoreBackMidConeDownTipIndex(),
+ .place_index = arm::ScoreBackMidConeDownTipPlacedIndex(),
+ .wrist_goal = 0.8,
+ .score_wrist_goal = 2.0,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kMidConeScoreRight, SpotSelectionHint::RIGHT},
+ {kMidConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::BACK,
+ .row_hint = RowSelectionHint::MIDDLE,
+ },
+ {
+ .index = arm::ScoreFrontMidConeDownTipIndex(),
+ .place_index = arm::ScoreFrontMidConeDownTipPlacedIndex(),
.wrist_goal = 0.0,
+ .score_wrist_goal = 1.4,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kMidConeScoreRight, SpotSelectionHint::RIGHT},
+ {kMidConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::MIDDLE,
+ },
+ {
+ .index = arm::ScoreFrontHighConeDownTipIndex(),
+ .place_index = arm::ScoreFrontHighConeDownTipPlacedIndex(),
+ .wrist_goal = 0.4,
+ .score_wrist_goal = 1.4,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kHighConeScoreRight, SpotSelectionHint::RIGHT},
+ {kHighConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::TOP,
+ },
+ {
+ .index = arm::ScoreFrontLowConeDownTipIndex(),
+ .wrist_goal = 2.8,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kLowConeScoreRight, SpotSelectionHint::RIGHT},
+ {kLowCube, SpotSelectionHint::MIDDLE},
+ {kLowConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::TOP,
+ },
+ {
+ .index = arm::ScoreBackMidConeUpIndex(),
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kMidConeScoreRight, SpotSelectionHint::RIGHT},
{kMidConeScoreLeft, SpotSelectionHint::LEFT}},
@@ -134,7 +195,7 @@
},
{
.index = arm::ScoreBackLowConeUpIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kLowConeScoreLeft, SpotSelectionHint::LEFT},
{kLowConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -143,7 +204,7 @@
},
{
.index = arm::ScoreFrontLowConeUpIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kLowConeScoreLeft, SpotSelectionHint::LEFT},
{kLowConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -152,8 +213,8 @@
},
{
.index = arm::ScoreBackMidConeDownBaseIndex(),
- .wrist_goal = 2.2,
- .score_wrist_goal = 0.0,
+ .wrist_goal = 2.5,
+ .score_wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kMidConeScoreLeft, SpotSelectionHint::LEFT},
{kMidConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -162,8 +223,7 @@
},
{
.index = arm::ScoreBackLowConeDownBaseIndex(),
- .wrist_goal = 0.0,
- .score_wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kLowConeScoreLeft, SpotSelectionHint::LEFT},
{kLowConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -172,7 +232,7 @@
},
{
.index = arm::ScoreFrontLowConeDownBaseIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kLowConeScoreLeft, SpotSelectionHint::LEFT},
{kLowConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -181,8 +241,8 @@
},
{
.index = arm::ScoreFrontMidConeDownBaseIndex(),
- .wrist_goal = 2.0,
- .score_wrist_goal = 0.0,
+ .wrist_goal = 2.6,
+ .score_wrist_goal = 0.2,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kMidConeScoreLeft, SpotSelectionHint::LEFT},
{kMidConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -191,8 +251,8 @@
},
{
.index = arm::ScoreFrontHighConeDownBaseIndex(),
- .wrist_goal = 2.0,
- .score_wrist_goal = 0.0,
+ .wrist_goal = 2.6,
+ .score_wrist_goal = 0.2,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kHighConeScoreLeft, SpotSelectionHint::LEFT},
{kHighConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -201,21 +261,21 @@
},
{
.index = arm::HPPickupFrontConeUpIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kHPConePickup}},
.side = Side::FRONT,
},
{
.index = arm::HPPickupBackConeUpIndex(),
- .wrist_goal = 0.4,
+ .wrist_goal = 0.5,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kHPConePickup}},
.side = Side::BACK,
},
{
.index = arm::ScoreFrontHighConeUpIndex(),
- .wrist_goal = 0.05,
+ .wrist_goal = kConeWrist + 0.05,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kHighConeScoreLeft, SpotSelectionHint::LEFT},
{kHighConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -224,7 +284,7 @@
},
{
.index = arm::ScoreFrontMidConeUpIndex(),
- .wrist_goal = 0.05,
+ .wrist_goal = kConeWrist + 0.05,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kMidConeScoreLeft, SpotSelectionHint::LEFT},
{kMidConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -233,14 +293,14 @@
},
{
.index = arm::GroundPickupBackCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kGroundPickupCube}},
.side = Side::BACK,
},
{
.index = arm::ScoreFrontMidCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kMidCube, SpotSelectionHint::MIDDLE}},
.side = Side::FRONT,
@@ -248,8 +308,7 @@
},
{
.index = arm::ScoreBackMidCubeIndex(),
- .wrist_goal = 0.6,
- .score_wrist_goal = 0.0,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kMidCube, SpotSelectionHint::MIDDLE}},
.side = Side::BACK,
@@ -257,7 +316,7 @@
},
{
.index = arm::ScoreFrontLowCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kLowCube, SpotSelectionHint::MIDDLE}},
.side = Side::FRONT,
@@ -265,7 +324,7 @@
},
{
.index = arm::ScoreBackLowCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kLowCube, SpotSelectionHint::MIDDLE}},
.side = Side::BACK,
@@ -273,7 +332,7 @@
},
{
.index = arm::ScoreFrontHighCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kHighCube, SpotSelectionHint::MIDDLE}},
.side = Side::FRONT,
@@ -281,8 +340,7 @@
},
{
.index = arm::ScoreBackHighCubeIndex(),
- .wrist_goal = 0.6,
- .score_wrist_goal = 0.0,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kHighCube, SpotSelectionHint::MIDDLE}},
.side = Side::BACK,
@@ -290,7 +348,7 @@
},
{
.index = arm::GroundPickupFrontCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kGroundPickupCube}},
.side = Side::FRONT,
@@ -333,11 +391,12 @@
RollerGoal roller_goal = RollerGoal::IDLE;
arm_goal_position_ = arm::NeutralIndex();
std::optional<double> score_wrist_goal = std::nullopt;
+ std::optional<double> place_index = std::nullopt;
if (data.IsPressed(kGroundPickupConeUp) || data.IsPressed(kHPConePickup)) {
roller_goal = RollerGoal::INTAKE_CONE_UP;
current_game_piece_ = GamePiece::CONE_UP;
- } else if (data.IsPressed(kGroundPickupConeDownBase)) {
+ } else if (data.IsPressed(kGroundPickupConeDown)) {
roller_goal = RollerGoal::INTAKE_CONE_DOWN;
current_game_piece_ = GamePiece::CONE_DOWN;
} else if (data.IsPressed(kGroundPickupCube)) {
@@ -345,8 +404,17 @@
current_game_piece_ = GamePiece::CUBE;
}
+ if (current_game_piece_ == GamePiece::CONE_DOWN ||
+ current_game_piece_ == GamePiece::CONE_TIP) {
+ if (data.IsPressed(kConeDownTip)) {
+ current_game_piece_ = GamePiece::CONE_TIP;
+ } else if (data.IsPressed(kConeDownBase)) {
+ current_game_piece_ = GamePiece::CONE_DOWN;
+ }
+ }
+
if (current_game_piece_ == GamePiece::CUBE) {
- wrist_goal = 0.6;
+ wrist_goal = kCubeWrist;
}
std::optional<RowSelectionHint> placing_row;
@@ -391,6 +459,7 @@
wrist_goal = current_setpoint_->wrist_goal;
arm_goal_position_ = current_setpoint_->index;
score_wrist_goal = current_setpoint_->score_wrist_goal;
+ place_index = current_setpoint_->place_index;
}
placing_row = current_setpoint_->row_hint;
@@ -407,9 +476,19 @@
// If we are supposed to dunk it, wait until we are close enough to
// spit.
if (std::abs(score_wrist_goal.value() -
- superstructure_status_fetcher_->wrist()->position()) <
+ superstructure_status_fetcher_->wrist()->goal_position()) <
0.1) {
- roller_goal = RollerGoal::SPIT;
+ if (place_index.has_value()) {
+ arm_goal_position_ = place_index.value();
+ if (arm_goal_position_ ==
+ superstructure_status_fetcher_->arm()->current_node() &&
+ superstructure_status_fetcher_->arm()->path_distance_to_go() <
+ 0.01) {
+ roller_goal = RollerGoal::SPIT;
+ }
+ } else {
+ roller_goal = RollerGoal::SPIT;
+ }
}
} else {
roller_goal = RollerGoal::SPIT;