commit 8ca44564acc06d96c7310ee637036b462a4df6e7
author Maxwell Henderson <mxwhenderson@gmail.com> Thu Feb 23 13:11:51 2023 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Thu Feb 23 21:37:56 2023 -0800
tree 8b80989b5a9e399e2fa73a29332a690210f435c7
parent 5938a8364b3c13b654060d1291852beee0d614f4

Add wrist logic and tests in superstructure

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ib71a65aacfc37858e8111a310dd3a97d308fb61c

y2023/constants.cc

diff --git a/y2023/constants.cc b/y2023/constants.cc
index e2e8522..0809f56 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -47,20 +47,22 @@
   roll_joint->zeroing.moving_buffer_size = 20;
   roll_joint->zeroing.allowable_encoder_error = 0.9;
 
-  wrist->zeroing_voltage = 3.0;
-  wrist->operating_voltage = 12.0;
-  wrist->zeroing_profile_params = {0.5, 3.0};
-  wrist->default_profile_params = {6.0, 30.0};
-  wrist->range = Values::kWristRange();
-  wrist->make_integral_loop =
+  wrist->subsystem_params.zeroing_voltage = 3.0;
+  wrist->subsystem_params.operating_voltage = 12.0;
+  wrist->subsystem_params.zeroing_profile_params = {0.5, 3.0};
+  wrist->subsystem_params.default_profile_params = {6.0, 30.0};
+  wrist->subsystem_params.range = Values::kWristRange();
+  wrist->subsystem_params.make_integral_loop =
       control_loops::superstructure::wrist::MakeIntegralWristLoop;
-  wrist->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
-  wrist->zeroing_constants.one_revolution_distance =
+  wrist->subsystem_params.zeroing_constants.average_filter_size =
+      Values::kZeroingSampleSize;
+  wrist->subsystem_params.zeroing_constants.one_revolution_distance =
       M_PI * 2.0 * constants::Values::kWristEncoderRatio();
-  wrist->zeroing_constants.zeroing_threshold = 0.0005;
-  wrist->zeroing_constants.moving_buffer_size = 20;
-  wrist->zeroing_constants.allowable_encoder_error = 0.9;
-  wrist->zeroing_constants.middle_position = Values::kWristRange().middle();
+  wrist->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
+  wrist->subsystem_params.zeroing_constants.moving_buffer_size = 20;
+  wrist->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
+  wrist->subsystem_params.zeroing_constants.middle_position =
+      Values::kWristRange().middle();
 
   switch (team) {
     // A set of constants for tests.
@@ -74,7 +76,8 @@
       roll_joint->zeroing.measured_absolute_position = 0.0;
       roll_joint->potentiometer_offset = 0.0;
 
-      wrist->zeroing_constants.measured_absolute_position = 0.0;
+      wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+          0.0;
 
       break;
 
@@ -93,7 +96,8 @@
           0.866186131631967 - 0.0256788357596952 + 0.18101759154572017 -
           0.0208958996127179;
 
-      wrist->zeroing_constants.measured_absolute_position = 0.0;
+      wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+          0.0;
 
       break;
 
@@ -107,7 +111,8 @@
       roll_joint->zeroing.measured_absolute_position = 0.0;
       roll_joint->potentiometer_offset = 0.0;
 
-      wrist->zeroing_constants.measured_absolute_position = 0.0;
+      wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+          0.0;
 
       break;
 
@@ -121,7 +126,8 @@
       roll_joint->zeroing.measured_absolute_position = 0.0;
       roll_joint->potentiometer_offset = 0.0;
 
-      wrist->zeroing_constants.measured_absolute_position = 0.0;
+      wrist->subsystem_params.zeroing_constants.measured_absolute_position =
+          0.0;
 
       break;