diff --git a/src/Constants.h b/src/Constants.h index da592f25..fb8f178f 100644 --- a/src/Constants.h +++ b/src/Constants.h @@ -123,13 +123,13 @@ extern const std::unordered_map STREAM_RFS; /** * A map that pairs each of the joints to its corresponding motor. + * (one-to-one pairs only) */ -constexpr frozen::unordered_map +constexpr frozen::unordered_map 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 index 662b7a25..972fdfce 100644 --- a/src/TunePID.cpp +++ b/src/TunePID.cpp @@ -43,7 +43,8 @@ const std::map 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 index c48299d0..e698fe2d 100644 --- 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 index efc47856..5a741e77 100644 --- 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 index 2024951e..58830f36 100644 --- 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::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( {{"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 index b51a576a..e24a986a 100644 --- 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({ {.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 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"}};