Temp change: Shoulder/Forearm controls to Wrist.

huskyroboticsteam · May 24, 2024 · 720b5c6 · 720b5c6
1 parent ee64910
commit 720b5c6
Showing 1 changed file with 10 additions and 2 deletions.
diff --git a/src/world_interface/kinematic_common_interface.cpp b/src/world_interface/kinematic_common_interface.cpp
@@ -171,7 +171,14 @@ double getJointPowerValue(types::jointid_t joint) {
 
 void setJointMotorPower(robot::types::jointid_t joint, double power) {
 	using robot::types::jointid_t;
-	if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) {
+
+	if (joint == jointid_t::shoulder) {
+		setMotorPower(robot::types::motorid_t::wristDiffRight, power);
+	} else if (joint == jointid_t::forearm) {
+		setMotorPower(robot::types::motorid_t::wristDiffLeft, power);
+	}
+
+	else if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) {
 		bool isIKMotor = std::find(Constants::arm::IK_MOTOR_JOINTS.begin(),
 								   Constants::arm::IK_MOTOR_JOINTS.end(),
 								   joint) != Constants::arm::IK_MOTOR_JOINTS.end();
@@ -190,7 +197,8 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) {
 					robot::getMotorPositionsRad(Constants::arm::IK_MOTORS).getData());
 			}
 		}
-	} else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) {
+	}
+	 else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) {
 		kinematics::DiffWristKinematics diffWristKinematics;
 		setJointPowerValue(joint, power);
 		kinematics::jointpos_t jointPwr2(getJointPowerValue(jointid_t::wristPitch),