Make zero straight up and fix all the positions.
This makes it so we can swap arms and everything works. Also, add a
scale factor to the distal to manange the difference in pulley sizes.
Change-Id: I974be95bf2c7493565ba9fe9c267e6e9a521f1b7
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/constants.cc b/y2023/constants.cc
index c8b75d1..616b777 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -35,8 +35,6 @@
arm_proximal->zeroing.allowable_encoder_error = 0.9;
arm_distal->zeroing.average_filter_size = Values::kZeroingSampleSize;
- arm_distal->zeroing.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kDistalEncoderRatio();
arm_distal->zeroing.zeroing_threshold = 0.0005;
arm_distal->zeroing.moving_buffer_size = 20;
arm_distal->zeroing.allowable_encoder_error = 0.9;
@@ -73,6 +71,8 @@
arm_distal->zeroing.measured_absolute_position = 0.0;
arm_distal->potentiometer_offset = 0.0;
+ arm_distal->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kDistalEncoderRatio();
roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
@@ -89,12 +89,16 @@
0.0820901660993467 - 0.0703733798337964 - 0.0294645384848748 -
0.577156175549626;
- arm_distal->zeroing.measured_absolute_position = 0.119544808434349;
+ arm_distal->zeroing.measured_absolute_position = 0.137511584277487;
arm_distal->potentiometer_offset =
0.436664933370656 + 0.49457213779426 + 6.78213223139724 -
0.0220711555235029 - 0.0162945074111813 + 0.00630344935527365 -
0.0164398318919943 - 0.145833494945215 + 0.234878799868491 +
- 0.125924230298394 + 0.147136306208754 - 0.69167546169753;
+ 0.125924230298394 + 0.147136306208754 - 0.69167546169753 -
+ 0.308761538844425;
+
+ arm_distal->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kDistalEncoderRatio();
roll_joint->zeroing.measured_absolute_position = 0.62315534539819;
roll_joint->potentiometer_offset =
@@ -103,7 +107,8 @@
0.0208958996127179 - 0.186395903925026 + 0.45801689548395 -
0.5935210745062 + 0.166256655718334 - 0.12591438680483 +
0.11972765117321 - 0.318724743041507) +
- 0.0201047336425017 - 1.0173426655158 - 0.186085272847293 - 0.0317706563397807;
+ 0.0201047336425017 - 1.0173426655158 - 0.186085272847293 -
+ 0.0317706563397807;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
2.27068625283861;
@@ -116,10 +121,15 @@
10.5178592988554 + 0.0944609125285876 - 0.00826532984625095 +
0.167359305216504;
- arm_distal->zeroing.measured_absolute_position = 0.0915283983599425;
+ arm_distal->zeroing.measured_absolute_position = 0.0698375027814462;
arm_distal->potentiometer_offset =
7.673132586937 - 0.0799284644472573 - 0.0323574039310657 +
- 0.0143810684138064 + 0.00945555248207735 + 0.452446388633863;
+ 0.0143810684138064 + 0.00945555248207735 + 0.452446388633863 +
+ 0.0194863477007102;
+
+ arm_distal->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kDistalEncoderRatio() *
+ 3.11964893168338 / 3.148;
roll_joint->zeroing.measured_absolute_position = 0.0722321007692069;
roll_joint->potentiometer_offset =
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 19e5a34..9aecb49 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -22,7 +22,7 @@
points['Neutral'] = np.array((np.pi, 0.0, 0.0))
points['GroundPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.07774334, 0.40, np.pi / 2.0, circular_index=1)
+ -1.07774334, 0.39, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -35,7 +35,7 @@
))
points['GroundPickupBackConeDownBase'] = to_theta_with_circular_index_and_roll(
- -1.11487594, 0.25, np.pi / 2.0, circular_index=1)
+ -1.11487594, 0.24, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -49,7 +49,7 @@
points[
'GroundPickupFrontConeDownBase'] = to_theta_with_circular_index_and_roll(
- 0.30, 0.24, -np.pi / 2.0, circular_index=0)
+ 0.30, 0.265, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -257,7 +257,7 @@
))
points['ScoreBackMidConeUp'] = to_theta_with_circular_index_and_roll(
- -1.45013, 1.04354, np.pi / 2.0, circular_index=1)
+ -1.45013, 1.00354, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -329,7 +329,7 @@
))
points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.1200539, 1.345, np.pi / 2.0, circular_index=0)
+ -1.1200539, 1.330, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -344,7 +344,7 @@
))
points['HPPickupFrontConeUp'] = np.array(
- (5.16514378449353, 1.26, -np.pi / 2.0))
+ (5.16514378449353, 1.25, -np.pi / 2.0))
# to_theta_with_circular_index_and_roll(
# 0.265749, 1.28332, -np.pi / 2.0, circular_index=1)
@@ -361,7 +361,7 @@
))
points['ScoreFrontHighConeUp'] = to_theta_with_circular_index_and_roll(
- 0.98810344, 1.37536719, -np.pi / 2.0, circular_index=0)
+ 0.98810344, 1.31536719, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -374,7 +374,7 @@
))
points['ScoreFrontMidConeUp'] = to_theta_with_circular_index_and_roll(
- 0.64, 1.03, -np.pi / 2.0, circular_index=0)
+ 0.64, 0.99, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -536,7 +536,7 @@
))
points['GroundPickupFrontConeUp'] = to_theta_with_circular_index_and_roll(
- 0.313099, 0.380, -np.pi / 2.0, circular_index=0)
+ 0.313099, 0.390, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 2f41c91..4f41d97 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -432,10 +432,10 @@
Values::kProximalEncoderRatio(), proximal_pot_translate,
true, values_->arm_proximal.potentiometer_offset);
frc971::PotAndAbsolutePositionT distal;
- CopyPosition(distal_encoder_, &distal,
- Values::kDistalEncoderCountsPerRevolution(),
- Values::kDistalEncoderRatio(), distal_pot_translate, true,
- values_->arm_distal.potentiometer_offset);
+ CopyPosition(
+ distal_encoder_, &distal, Values::kDistalEncoderCountsPerRevolution(),
+ values_->arm_distal.zeroing.one_revolution_distance / (M_PI * 2.0),
+ distal_pot_translate, true, values_->arm_distal.potentiometer_offset);
frc971::PotAndAbsolutePositionT roll_joint;
CopyPosition(roll_joint_encoder_, &roll_joint,
Values::kRollJointEncoderCountsPerRevolution(),