commit 5c47a46ea78b67b771d622021926c43c946acdb1
author Maxwell Henderson <mxwhenderson@gmail.com> Sat Feb 25 14:40:44 2023 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 04 23:23:31 2023 -0800
tree 525ae0161bc00cd0a5335088018e2e646316229c
parent 07b95a50ae00cbe63658fbc9a35d90c80f691c76

Move arm constants to constants.h

Change-Id: I0fe6de83a62adc9e330ba55025cb88920a8e43e2
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2023/control_loops/superstructure/arm/arm.cc

diff --git a/y2023/control_loops/superstructure/arm/arm.cc b/y2023/control_loops/superstructure/arm/arm.cc
index fd5d83f..27a1450 100644
--- a/y2023/control_loops/superstructure/arm/arm.cc
+++ b/y2023/control_loops/superstructure/arm/arm.cc
@@ -26,8 +26,9 @@
       distal_offset_(0.0),
       roll_joint_offset_(0.0),
       alpha_unitizer_((::Eigen::DiagonalMatrix<double, 3>().diagonal()
-                           << (1.0 / kAlpha0Max()),
-                       (1.0 / kAlpha1Max()), (1.0 / kAlpha2Max()))
+                           << (1.0 / constants::Values::kArmAlpha0Max()),
+                       (1.0 / constants::Values::kArmAlpha1Max()),
+                       (1.0 / constants::Values::kArmAlpha2Max()))
                           .finished()),
       dynamics_(kArmConstants),
       close_enough_for_full_power_(false),
@@ -36,7 +37,8 @@
       hybrid_roll_joint_loop_(roll::MakeIntegralHybridRollLoop()),
       arm_ekf_(&dynamics_),
       search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
-                                    kVMax(), &hybrid_roll_joint_loop_)),
+                                    constants::Values::kArmVMax(),
+                                    &hybrid_roll_joint_loop_)),
       // Go to the start of the first trajectory.
       follower_(&dynamics_, &hybrid_roll_joint_loop_, NeutralPoint()),
       points_(PointList()),
@@ -117,7 +119,7 @@
     *roll_joint_output = 0.0;
   }
 
-  arm_ekf_.Correct(Y_arm, kDt());
+  arm_ekf_.Correct(Y_arm, constants::Values::kArmDt());
   roll_joint_loop_.Correct(Y_roll_joint);
 
   if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= 0.05 &&
@@ -276,16 +278,21 @@
 
   const double max_operating_voltage =
       close_enough_for_full_power_
-          ? kOperatingVoltage()
-          : (state_ == ArmState::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
+          ? constants::Values::kArmOperatingVoltage()
+          : (state_ == ArmState::GOTO_PATH
+                 ? constants::Values::kArmGotoPathVMax()
+                 : constants::Values::kArmPathlessVMax());
   ::Eigen::Matrix<double, 9, 1> X_hat;
   X_hat.block<6, 1>(0, 0) = arm_ekf_.X_hat();
   X_hat.block<3, 1>(6, 0) = roll_joint_loop_.X_hat();
 
-  follower_.Update(X_hat, disable, kDt(), vmax_, max_operating_voltage);
+  follower_.Update(X_hat, disable, constants::Values::kArmDt(), vmax_,
+                   max_operating_voltage);
+  AOS_LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
 
-  arm_ekf_.Predict(follower_.U().head<2>(), kDt());
-  roll_joint_loop_.UpdateObserver(follower_.U().tail<1>(), kDtDuration());
+  arm_ekf_.Predict(follower_.U().head<2>(), constants::Values::kArmDt());
+  roll_joint_loop_.UpdateObserver(follower_.U().tail<1>(),
+                                  constants::Values::kArmDtDuration());
 
   flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
       proximal_estimator_state_offset =
@@ -329,11 +336,14 @@
 
   if (!disable) {
     *proximal_output = ::std::max(
-        -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(0)));
+        -constants::Values::kArmOperatingVoltage(),
+        ::std::min(constants::Values::kArmOperatingVoltage(), follower_.U(0)));
     *distal_output = ::std::max(
-        -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
+        -constants::Values::kArmOperatingVoltage(),
+        ::std::min(constants::Values::kArmOperatingVoltage(), follower_.U(1)));
     *roll_joint_output = ::std::max(
-        -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(2)));
+        -constants::Values::kArmOperatingVoltage(),
+        ::std::min(constants::Values::kArmOperatingVoltage(), follower_.U(2)));
   }
 
   status_builder.add_path_distance_to_go(follower_.path_distance_to_go());