Add joints for swerve steering

huskyroboticsteam · Jul 3, 2024 · 3f81b82 · 3f81b82
1 parent 7c81841
commit 3f81b82
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 10 deletions.
diff --git a/src/Constants.h b/src/Constants.h
@@ -132,15 +132,21 @@ extern const std::unordered_map<robot::types::CameraID, int> STREAM_RFS;
 /**
  * A map that pairs each of the joints to its corresponding motor.
  */
-constexpr frozen::unordered_map<robot::types::jointid_t, robot::types::motorid_t, 7>
-	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}};
+constexpr auto JOINT_MOTOR_MAP =
+	frozen::make_unordered_map<robot::types::jointid_t, robot::types::motorid_t>(
+		{{robot::types::jointid_t::frontLeftSwerve, robot::types::motorid_t::frontLeftSwerve},
+		 {robot::types::jointid_t::frontRightSwerve,
+		  robot::types::motorid_t::frontRightSwerve},
+		 {robot::types::jointid_t::rearLeftSwerve, robot::types::motorid_t::rearLeftSwerve},
+		 {robot::types::jointid_t::rearRightSwerve, robot::types::motorid_t::rearRightSwerve},
+		 {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}});
 
 // Arm inverse kinematics
 namespace arm {

diff --git a/src/world_interface/data.h b/src/world_interface/data.h
@@ -82,6 +82,10 @@ enum class mountedperipheral_t {
 };
 
 enum class jointid_t {
+	frontLeftSwerve,
+	frontRightSwerve,
+	rearLeftSwerve,
+	rearRightSwerve,
 	armBase,
 	shoulder,
 	elbow,
@@ -99,7 +103,11 @@ constexpr auto all_jointid_t = frozen::make_unordered_set<jointid_t>(
 	 jointid_t::ikUp});
 
 constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, jointid_t>(
-	{{"armBase", jointid_t::armBase},
+	{{"frontLeftSwerve", jointid_t::frontLeftSwerve},
+	 {"frontRightSwerve", jointid_t::frontRightSwerve},
+	 {"rearLeftSwerve", jointid_t::rearLeftSwerve},
+	 {"rearRightSwerve", jointid_t::rearRightSwerve},
+	 {"armBase", jointid_t::armBase},
 	 {"shoulder", jointid_t::shoulder},
 	 {"elbow", jointid_t::elbow},
 	 {"forearm", jointid_t::forearm},