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>
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());
diff --git a/y2023/control_loops/superstructure/arm/arm.h b/y2023/control_loops/superstructure/arm/arm.h
index 6b7da54..216b89d 100644
--- a/y2023/control_loops/superstructure/arm/arm.h
+++ b/y2023/control_loops/superstructure/arm/arm.h
@@ -23,26 +23,6 @@
public:
Arm(std::shared_ptr<const constants::Values> values);
- // if true, tune down all the constants for testing.
- static constexpr bool kGrannyMode() { return false; }
-
- // the operating voltage.
- static constexpr double kOperatingVoltage() {
- return kGrannyMode() ? 6.0 : 12.0;
- }
- static constexpr double kVMax() { return kGrannyMode() ? 4.0 : 9.5; }
- static constexpr double kPathlessVMax() { return 4.5; }
- static constexpr double kGotoPathVMax() { return 4.5; }
-
- static constexpr double kDt() { return 0.00505; }
- static constexpr std::chrono::nanoseconds kDtDuration() {
- return std::chrono::duration_cast<std::chrono::nanoseconds>(
- std::chrono::duration<double>(kDt()));
- }
- static constexpr double kAlpha0Max() { return kGrannyMode() ? 15.0 : 15.0; }
- static constexpr double kAlpha1Max() { return kGrannyMode() ? 10.0 : 10.0; }
- static constexpr double kAlpha2Max() { return kGrannyMode() ? 90.0 : 90.0; }
-
flatbuffers::Offset<superstructure::ArmStatus> Iterate(
const ::aos::monotonic_clock::time_point /*monotonic_now*/,
const uint32_t *unsafe_goal, const superstructure::ArmPosition *position,
@@ -89,7 +69,7 @@
const ::Eigen::DiagonalMatrix<double, 3> alpha_unitizer_;
- double vmax_ = kVMax();
+ double vmax_ = constants::Values::kArmVMax();
frc971::control_loops::arm::Dynamics dynamics_;
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 3252030..2a67adf 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -127,8 +127,7 @@
double roll_joint_velocity() const { return roll_joint_loop_.X_hat(1, 0); }
void Simulate(::Eigen::Matrix<double, 3, 1> U) {
- constexpr double voltage_check =
- superstructure::arm::Arm::kOperatingVoltage();
+ constexpr double voltage_check = constants::Values::kArmOperatingVoltage();
AOS_CHECK_LE(::std::abs(U(0)), voltage_check);
AOS_CHECK_LE(::std::abs(U(1)), voltage_check);