From 31d57a21d1e5d72875d1f429eb234d035901ae7a Mon Sep 17 00:00:00 2001
From: Geeoon Chung <geeoon13@gmail.com>
Date: Sat, 26 Oct 2024 11:49:06 -0700
Subject: [PATCH] Adds PID max power to CANMotors

---
 src/CAN/CANMotor.cpp |  7 +++++++
 src/CAN/CANMotor.h   | 18 ++++++++++++++++--
 src/Constants.h      | 14 ++++++--------
 src/TunePID.cpp      |  5 +++--
 4 files changed, 32 insertions(+), 12 deletions(-)

diff --git a/src/CAN/CANMotor.cpp b/src/CAN/CANMotor.cpp
index a68eda07c..3e4355752 100644
--- a/src/CAN/CANMotor.cpp
+++ b/src/CAN/CANMotor.cpp
@@ -92,6 +92,13 @@ void setMotorPIDConstants(deviceserial_t serial, int32_t kP, int32_t kI, int32_t
 	std::this_thread::sleep_for(1ms);
 }
 
+void setMotorPIDMaxPower(deviceserial_t serial, uint16_t maxPower) {
+	CANPacket p;
+	auto motorGroupCode = static_cast<uint8_t>(devicegroup_t::motor);
+	AssembleMaxPIDPWMPacket(&p, motorGroupCode, serial, maxPower);
+	sendCANPacket(p);
+}
+
 void setMotorMode(deviceserial_t serial, motormode_t mode) {
 	CANPacket p;
 	AssembleModeSetPacket(&p, static_cast<uint8_t>(devicegroup_t::motor), serial,
diff --git a/src/CAN/CANMotor.h b/src/CAN/CANMotor.h
index 7c092227e..6e20a647e 100644
--- a/src/CAN/CANMotor.h
+++ b/src/CAN/CANMotor.h
@@ -16,11 +16,17 @@
 namespace can::motor {
 
 /** @brief The possible motor modes. */
-enum class motormode_t { pwm = MOTOR_UNIT_MODE_PWM, pid = MOTOR_UNIT_MODE_PID };
+enum class motormode_t {
+	pwm = MOTOR_UNIT_MODE_PWM,
+	pid = MOTOR_UNIT_MODE_PID
+};
 
 /** @brief The supported motor position sensors. */
 struct sensor_t {
-	enum { encoder = 0, potentiometer = 1 };
+	enum {
+		encoder = 0,
+		potentiometer = 1
+	};
 };
 
 /**
@@ -136,6 +142,14 @@ void setMotorPower(deviceserial_t serial, int16_t power);
  */
 void setMotorPIDTarget(deviceserial_t serial, int32_t target);
 
+/**
+ * @brief Set the maximum power allowed for a motor when running PID.
+ *
+ * @param serial The CAN serial number of the motor board.
+ * @param maxPower The maximum power allowed for the motor.
+ */
+void setMotorPIDMaxPower(deviceserial_t serial, uint16_t maxPower);
+
 /**
  * @brief Set the angle of the PCA servo
  *
diff --git a/src/Constants.h b/src/Constants.h
index 01c3e9530..aa5bf6957 100644
--- a/src/Constants.h
+++ b/src/Constants.h
@@ -53,15 +53,13 @@ extern const double MAX_DTHETA;
 
 // TODO: We need to recalibrate the camera, since we replaced it with a different one.
 // TODO: rename cameras (in MC as well) as appropriate
-extern const char* MAST_CAMERA_CONFIG_PATH;
 extern const robot::types::CameraID MAST_CAMERA_ID;
-
-extern const char* FOREARM_CAMERA_CONFIG_PATH;
-extern const robot::types::CameraID FOREARM_CAMERA_ID;
-
-extern const char* HAND_CAMERA_CONFIG_PATH;
+extern const robot::types::CameraID WRIST_CAMERA_ID;
 extern const robot::types::CameraID HAND_CAMERA_ID;
 
+extern const std::unordered_map<robot::types::CameraID, std::string> CAMERA_CONFIG_PATHS;
+extern const std::unordered_map<std::string, robot::types::CameraID> CAMERA_NAME_TO_ID;
+
 extern const uint16_t WS_SERVER_PORT;
 
 /**
@@ -186,8 +184,8 @@ constexpr frozen::unordered_map<robot::types::motorid_t, std::pair<int, int>, IK
  * Map from motor ids to segment length in meters
  */
 constexpr frozen::unordered_map<robot::types::motorid_t, double, IK_MOTORS.size()>
-	SEGMENT_LENGTHS{{robot::types::motorid_t::shoulder, 0.3848608},
-					{robot::types::motorid_t::elbow, 0.461264}};
+	SEGMENT_LENGTHS{{robot::types::motorid_t::shoulder, 0.394},
+					{robot::types::motorid_t::elbow, 0.749}};
 } // namespace arm
 
 namespace autonomous {
diff --git a/src/TunePID.cpp b/src/TunePID.cpp
index 9f6675d3d..1d18999ee 100644
--- a/src/TunePID.cpp
+++ b/src/TunePID.cpp
@@ -18,7 +18,8 @@ extern "C" {
 }
 
 enum class targetmode_t {
-	step, sinusoidal
+	step,
+	sinusoidal
 };
 
 using namespace robot::types;
@@ -159,7 +160,7 @@ int main(int argc, char** argv) {
 		if (mode == targetmode_t::step) {
 			prescaled_target = round(prescaled_target);
 		}
-		angle_target = (int32_t) round(amplitude * prescaled_target) + starting_angle;
+		angle_target = (int32_t)round(amplitude * prescaled_target) + starting_angle;
 
 		can::motor::setMotorPIDTarget(serial, angle_target);