Moved changes to the new control_interface

huskyroboticsteam · Jun 21, 2024 · 8d6414b · 8d6414b
1 parent 25665b0
commit 8d6414b
Show file tree

Hide file tree

Showing 7 changed files with 35 additions and 16 deletions.
diff --git a/src/Constants.h b/src/Constants.h
@@ -123,13 +123,13 @@ extern const std::unordered_map<robot::types::CameraID, int> STREAM_RFS;
 
 /**
  * A map that pairs each of the joints to its corresponding motor.
+ * (one-to-one pairs only)
  */
-constexpr frozen::unordered_map<robot::types::jointid_t, robot::types::motorid_t, 7>
+constexpr frozen::unordered_map<robot::types::jointid_t, robot::types::motorid_t, 6>
 	JOINT_MOTOR_MAP{{robot::types::jointid_t::armBase, robot::types::motorid_t::armBase},
 					{robot::types::jointid_t::shoulder, robot::types::motorid_t::shoulder},
 					{robot::types::jointid_t::elbow, robot::types::motorid_t::elbow},
 					{robot::types::jointid_t::forearm, robot::types::motorid_t::forearm},
-					{robot::types::jointid_t::wrist, robot::types::motorid_t::wrist},
 					{robot::types::jointid_t::hand, robot::types::motorid_t::hand},
 					{robot::types::jointid_t::activeSuspension,
 					 robot::types::motorid_t::activeSuspension}};

diff --git a/src/TunePID.cpp b/src/TunePID.cpp
@@ -43,7 +43,8 @@ const std::map<motorid_t, std::string> motorNameMap = {
 	{motorid_t::shoulder, "shoulder"},
 	{motorid_t::elbow, "elbow"},
 	{motorid_t::forearm, "forearm"},
-	{motorid_t::wrist, "wrist"},
+	{motorid_t::wristDiffRight, "wristDiffRight"},
+	{motorid_t::wristDiffLeft, "wristDiffLeft"},
 	{motorid_t::hand, "hand"},
 	{motorid_t::activeSuspension, "activeSuspension"}};
 

diff --git a/src/control_interface.cpp b/src/control_interface.cpp
@@ -164,6 +164,12 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) {
 				}
 			}
 		}
+	} else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) {
+		kinematics::DiffWristKinematics diffWristKinematics;
+		setJointPowerValue(joint, power);
+		kinematics::jointpos_t jointPwr(getJointPowerValue(jointid_t::wristPitch),
+										 getJointPowerValue(jointid_t::wristRoll));
+		kinematics::gearpos_t gearPwr = diffWristKinematics.jointPowerToGearPower(jointPwr); 
 	} else {
 		LOG_F(WARNING, "setJointPower called for currently unsupported joint %s",
 			  util::to_string(joint).c_str());

diff --git a/src/world_interface/data.cpp b/src/world_interface/data.cpp
@@ -40,8 +40,10 @@ std::string to_string(robot::types::jointid_t joint) {
 			return "forearm";
 		case jointid_t::hand:
 			return "hand";
-		case jointid_t::wrist:
-			return "wrist";
+		case jointid_t::wristPitch:
+			return "wristPitch";
+		case jointid_t::wristRoll:
+			return "wristRoll";
 		case jointid_t::activeSuspension:
 			return "activeSuspension";
 		case jointid_t::ikUp:

diff --git a/src/world_interface/data.h b/src/world_interface/data.h
@@ -67,7 +67,8 @@ enum class motorid_t {
 	shoulder,
 	elbow,
 	forearm,
-	wrist,
+	wristDiffRight,
+	wristDiffLeft,
 	hand,
 	activeSuspension
 };
@@ -86,7 +87,8 @@ enum class jointid_t {
 	shoulder,
 	elbow,
 	forearm,
-	wrist,
+	wristPitch,
+	wristRoll,
 	hand,
 	activeSuspension,
 	ikForward,
@@ -95,15 +97,16 @@ enum class jointid_t {
 
 constexpr auto all_jointid_t = frozen::make_unordered_set<jointid_t>(
 	{jointid_t::armBase, jointid_t::shoulder, jointid_t::elbow, jointid_t::forearm,
-	 jointid_t::wrist, jointid_t::hand, jointid_t::activeSuspension, jointid_t::ikForward,
-	 jointid_t::ikUp});
+	 jointid_t::wristRoll, jointid_t::wristPitch, jointid_t::hand, jointid_t::activeSuspension,
+	 jointid_t::ikForward, jointid_t::ikUp});
 
 constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, jointid_t>(
 	{{"armBase", jointid_t::armBase},
 	 {"shoulder", jointid_t::shoulder},
 	 {"elbow", jointid_t::elbow},
 	 {"forearm", jointid_t::forearm},
-	 {"wrist", jointid_t::wrist},
+	 {"wristPitch", jointid_t::wristPitch},
+	 {"wristRoll", jointid_t::wristRoll},
 	 {"hand", jointid_t::hand},
 	 {"activeSuspension", jointid_t::activeSuspension},
 	 {"ikForward", jointid_t::ikForward},

diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h
@@ -79,8 +79,10 @@ constexpr auto potMotors = frozen::make_unordered_map<motorid_t, potparams_t>({
 	 {.adc_lo = 123, .mdeg_lo = -200 * 1000, .adc_hi = 456, .mdeg_hi = 200 * 1000}},
 	{motorid_t::forearm,
 	 {.adc_lo = 1208, .mdeg_lo = -180 * 1000, .adc_hi = 841, .mdeg_hi = 180 * 1000}},
-	{motorid_t::wrist,
-	 {.adc_lo = 123, .mdeg_lo = -100 * 1000, .adc_hi = 456, .mdeg_hi = 100 * 1000}},
+	{motorid_t::wristDiffLeft,
+	 {.adc_lo = 0, .mdeg_lo = -100 * 0, .adc_hi = 0, .mdeg_hi = 100 * 0}},
+	{motorid_t::wristDiffRight,
+	 {.adc_lo = 0, .mdeg_lo = -100 * 0, .adc_hi = 0, .mdeg_hi = 100 * 0}},
 	{motorid_t::activeSuspension,
 	 {.adc_lo = 251, .mdeg_lo = -19 * 1000, .adc_hi = 1645, .mdeg_hi = 31 * 1000}},
 	{motorid_t::frontLeftSwerve,
@@ -107,7 +109,9 @@ constexpr auto motorSerialIDMap = frozen::make_unordered_map<motorid_t, can::dev
 	 {motorid_t::shoulder, DEVICE_SERIAL_MOTOR_SHOULDER},
 	 {motorid_t::elbow, DEVICE_SERIAL_MOTOR_ELBOW},
 	 {motorid_t::forearm, DEVICE_SERIAL_MOTOR_FOREARM},
-	 {motorid_t::wrist, DEVICE_SERIAL_MOTOR_WRIST},
+	 // TODO: change serial IDs for differentials appropriately.
+	 {motorid_t::wristDiffLeft, DEVICE_SERIAL_MOTOR_WRIST},
+	 {motorid_t::wristDiffRight, DEVICE_SERIAL_MOTOR_WRIST},
 	 {motorid_t::hand, DEVICE_SERIAL_MOTOR_HAND},
 	 {motorid_t::activeSuspension, DEVICE_SERIAL_LINEAR_ACTUATOR}});
 
@@ -129,7 +133,8 @@ constexpr auto positive_pwm_scales =
 												   {motorid_t::shoulder, 1},
 												   {motorid_t::elbow, 1},
 												   {motorid_t::forearm, -0.65},
-												   {motorid_t::wrist, 0.1},
+												   {motorid_t::wristDiffLeft, 0.1},
+												   {motorid_t::wristDiffRight, 0.1},
 												   {motorid_t::frontLeftWheel, 0.7},
 												   {motorid_t::frontRightWheel, 0.7},
 												   {motorid_t::rearLeftWheel, 0.7},
@@ -149,7 +154,8 @@ constexpr auto negative_pwm_scales =
 												   {motorid_t::shoulder, 1},
 												   {motorid_t::elbow, 1},
 												   {motorid_t::forearm, -0.65},
-												   {motorid_t::wrist, 0.1},
+												   {motorid_t::wristDiffLeft, 0.1},
+												   {motorid_t::wristDiffRight, 0.1},
 												   {motorid_t::frontLeftWheel, 0.7},
 												   {motorid_t::frontRightWheel, 0.7},
 												   {motorid_t::rearLeftWheel, 0.7},

diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp
@@ -37,7 +37,8 @@ const std::map<motorid_t, std::string> motorNameMap = {
 	{motorid_t::shoulder, "shoulder"},
 	{motorid_t::elbow, "elbow"},
 	{motorid_t::forearm, "forearm"},
-	{motorid_t::wrist, "wrist"},
+	{motorid_t::wristDiffLeft, "wristDiffLeft"},
+	{motorid_t::wristDiffRight, "wristDiffRight"},
 	{motorid_t::hand, "hand"},
 	{motorid_t::activeSuspension, "activeSuspension"}};