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());