Adds PID max power to CANMotors

huskyroboticsteam · Oct 26, 2024 · 31d57a2 · 31d57a2
1 parent d0c3ad0
commit 31d57a2
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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);