From 7630036179cdf956105d37834c16ba37ec121085 Mon Sep 17 00:00:00 2001
From: DrDab <duvictor514@gmail.com>
Date: Thu, 28 Sep 2023 18:22:42 -0700
Subject: [PATCH] Move safety factor constant into Constants

---
 src/Constants.h | 15 ++++-----------
 src/Globals.cpp |  2 +-
 2 files changed, 5 insertions(+), 12 deletions(-)

diff --git a/src/Constants.h b/src/Constants.h
index 27aae6327..d7f2c4e00 100644
--- a/src/Constants.h
+++ b/src/Constants.h
@@ -56,17 +56,6 @@ constexpr double MAX_WHEEL_VEL = WHEEL_RADIUS * MAX_DRIVE_PWM / PWM_PER_RAD_PER_
 const double MAX_DTHETA = kinematics::DiffDriveKinematics(EFF_WHEEL_BASE)
 							  .wheelVelToRobotVel(-MAX_WHEEL_VEL, MAX_WHEEL_VEL)(2);
 
-// Joint limits
-// TODO: make sure these are still accurate with the new arm.
-constexpr double ARM_BASE_MIN = -M_PI / 2;
-constexpr double ARM_BASE_MAX = M_PI / 2;
-// constexpr double SHOULDER_MIN = M_PI / 2; // TODO mechanical problem with the moon gear.
-//                                             Use this value during actual rover operation.
-constexpr double SHOULDER_MIN = 0.0;
-constexpr double SHOULDER_MAX = M_PI * 5. / 6.; // Can't quite lie flat
-constexpr double ELBOW_MIN = 0.0;
-constexpr double ELBOW_MAX = M_PI * 29. / 30.; // I think this should prevent self-collisions
-
 // TODO: We need to recalibrate the camera, since we replaced it with a different one.
 // TODO: rename cameras (in MC as well) as appropriate
 constexpr const char* AR_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml";
@@ -146,6 +135,10 @@ constexpr frozen::unordered_map<robot::types::jointid_t, robot::types::motorid_t
 
 // Arm inverse kinematics
 namespace arm {
+/** 
+ * Percentage of fully extended overall arm length to limit arm extension within.
+ */
+constexpr double SAFETY_FACTOR = 0.95;
 
 /**
  * Maximum commanded end-effector velocity, in m/s
diff --git a/src/Globals.cpp b/src/Globals.cpp
index de639348f..a1e20fe22 100644
--- a/src/Globals.cpp
+++ b/src/Globals.cpp
@@ -41,6 +41,6 @@ robot::types::mountedperipheral_t mountedPeripheral = robot::types::mountedperip
 const kinematics::PlanarArmKinematics<Constants::arm::IK_MOTORS.size()>
 	planarArmKinematics(getSegLens(), getJointLimits(true), getJointLimits(false),
 						IK_SOLVER_THRESH, IK_SOLVER_MAX_ITER);
-control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics, 0.95);
+control::PlanarArmController<2> planarArmController({0, 0}, planarArmKinematics, Constants::arm::SAFETY_FACTOR);
 std::atomic<bool> armIKEnabled = false;
 } // namespace Globals