Replaced single wrist with wrist differential.

rover-config/50-rover-cameras.rules

@@ -6,11 +6,11 @@
 #
 # Install this in /etc/udev/rules.d
 
-# Mast camera
-SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", KERNELS=="1-2.1", SYMLINK+="video20"
+# Hand camera
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="0c45", ATTRS{idProduct}=="6369", KERNELS=="1-2.1", SYMLINK+="video20"
 
 # Forearm camera
-# SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9601", KERNELS=="1-2.1", SYMLINK+="video30"
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="9230", KERNELS=="1-2.1", SYMLINK+="video30"
 
-# Hand camera
+# Mast camera
 SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", KERNELS=="1-2.4.4", SYMLINK+="video40"

src/CAN/CANMotor.cpp

@@ -44,6 +44,7 @@ void initPotentiometer(deviceserial_t serial, int32_t posLo, int32_t posHi, uint
 	auto group = static_cast<uint8_t>(devicegroup_t::motor);
 	AssemblePotHiSetPacket(&packet, group, serial, adcHi, posHi);
 	sendCANPacket(packet);
+	std::this_thread::sleep_for(1ms);
 	AssemblePotLoSetPacket(&packet, group, serial, adcLo, posLo);
 	sendCANPacket(packet);
 	if (telemetryPeriod) {
@@ -72,7 +73,7 @@ void setLimitSwitchLimits(deviceserial_t serial, int32_t lo, int32_t hi) {
 	// 0 is low limit, 1 is high limit.
 	AssembleLimSwEncoderBoundPacket(&p, motorGroupCode, serial, 0, lo);
 	sendCANPacket(p);
-
+	std::this_thread::sleep_for(1ms);
 	AssembleLimSwEncoderBoundPacket(&p, motorGroupCode, serial, 1, hi);
 	sendCANPacket(p);
 }
@@ -82,11 +83,13 @@ void setMotorPIDConstants(deviceserial_t serial, int32_t kP, int32_t kI, int32_t
 	auto motorGroupCode = static_cast<uint8_t>(devicegroup_t::motor);
 	AssemblePSetPacket(&p, motorGroupCode, serial, kP);
 	sendCANPacket(p);
+	std::this_thread::sleep_for(1ms);
 	AssembleISetPacket(&p, motorGroupCode, serial, kI);
 	sendCANPacket(p);
+	std::this_thread::sleep_for(1ms);
 	AssembleDSetPacket(&p, motorGroupCode, serial, kD);
 	sendCANPacket(p);
-	std::this_thread::sleep_for(1000us);
+	std::this_thread::sleep_for(1ms);
 }
 
 void setMotorMode(deviceserial_t serial, motormode_t mode) {

src/CMakeLists.txt

@@ -94,7 +94,7 @@ find_package(nlohmann_json 3.2.0 REQUIRED)
 FetchContent_Declare(
     HindsightCAN
     GIT_REPOSITORY https://github.com/huskyroboticsteam/HindsightCAN.git
-    GIT_TAG 7397787fbb04687bf03c8fbac9a1db56f245fc9a
+    GIT_TAG 45b1544f20e7ba6dd0db19fb97e947430031fc6a
 )
 FetchContent_MakeAvailable(HindsightCAN)
 

src/Constants.h

@@ -131,6 +131,7 @@ 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 auto JOINT_MOTOR_MAP =
 	frozen::make_unordered_map<robot::types::jointid_t, robot::types::motorid_t>(
@@ -143,7 +144,6 @@ constexpr auto JOINT_MOTOR_MAP =
 		 {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}});

src/Globals.cpp

@@ -46,6 +46,7 @@ net::websocket::SingleClientWSServer websocketServer("DefaultServer",
 													 Constants::WS_SERVER_PORT);
 std::atomic<bool> AUTONOMOUS = false;
 robot::types::mountedperipheral_t mountedPeripheral = robot::types::mountedperipheral_t::none;
+const kinematics::DiffWristKinematics wristKinematics;
 control::PlanarArmController<2> planarArmController(createArmKinematics(),
 													Constants::arm::SAFETY_FACTOR);
 control::SwerveController swerveController(Constants::ROBOT_WIDTH, Constants::ROBOT_LENGTH,

src/Globals.h

@@ -3,6 +3,7 @@
 #include "control/PlanarArmController.h"
 #include "control/SwerveController.h"
 #include "kinematics/ArmKinematics.h"
+#include "kinematics/DiffWristKinematics.h"
 #include "network/websocket/WebSocketServer.h"
 #include "world_interface/data.h"
 
@@ -29,6 +30,7 @@ extern net::websocket::SingleClientWSServer websocketServer;
 extern std::atomic<bool> AUTONOMOUS;
 extern robot::types::mountedperipheral_t mountedPeripheral;
 extern control::PlanarArmController<2> planarArmController;
+extern const kinematics::DiffWristKinematics wristKinematics;
 extern control::SwerveController swerveController;
 extern std::atomic<bool> armIKEnabled;
 } // namespace Globals

src/TunePID.cpp

@@ -47,7 +47,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"}};
 

src/control/SwerveController.cpp

@@ -138,6 +138,10 @@ void SwerveController::setOverride(bool override) {
 	override_steer_check = override;
 }
 
+bool SwerveController::isOverridden() const {
+	return override_steer_check;
+}
+
 namespace util {
 std::string to_string(control::DriveMode mode) {
 	using control::DriveMode;

src/control/SwerveController.h

@@ -134,6 +134,13 @@ class SwerveController {
 	 */
 	void setOverride(bool override);
 
+	/**
+	 * @brief Check the override flag for wheel rotation checking.
+	 *
+	 * @return True iff the rotation checking is overridden.
+	 */
+	bool isOverridden() const;
+
 	/**
 	 * @brief Get the current drive mode of the controller.
 	 *

src/control_interface.cpp

@@ -40,18 +40,22 @@ double setCmdVel(double dtheta, double dx) {
 		return 0;
 	}
 
-	control::swerve_rots_t curr_wheel_rots;
-	try {
-		curr_wheel_rots = {robot::getMotorPos(motorid_t::frontLeftSwerve).getData(),
-						   robot::getMotorPos(motorid_t::frontRightSwerve).getData(),
-						   robot::getMotorPos(motorid_t::rearLeftSwerve).getData(),
-						   robot::getMotorPos(motorid_t::rearRightSwerve).getData()};
-	} catch (const bad_datapoint_access& e) {
-		LOG_F(WARNING, "Invalid steer motor position(s)!");
-		return 0;
+	if (!Globals::swerveController.isOverridden()) {
+		try {
+			control::swerve_rots_t curr_wheel_rots = {
+				robot::getMotorPos(motorid_t::frontLeftSwerve).getData(),
+				robot::getMotorPos(motorid_t::frontRightSwerve).getData(),
+				robot::getMotorPos(motorid_t::rearLeftSwerve).getData(),
+				robot::getMotorPos(motorid_t::rearRightSwerve).getData()};
+			if (!Globals::swerveController.checkWheelRotation(DriveMode::Normal,
+															  curr_wheel_rots)) {
+				return 0;
+			}
+		} catch (const bad_datapoint_access& e) {
+			LOG_F(WARNING, "Invalid steer motor position(s)!");
+			return 0;
+		}
 	}
-	if (!Globals::swerveController.checkWheelRotation(DriveMode::Normal, curr_wheel_rots))
-		return 0;
 
 	wheelvel_t wheelVels = driveKinematics().robotVelToWheelVel(dx, dtheta);
 	double lPWM = wheelVels.lVel / Constants::MAX_WHEEL_VEL;
@@ -121,27 +125,7 @@ void setJointPos(robot::types::jointid_t joint, int32_t targetPos) {
 	using robot::types::jointid_t;
 	if (Constants::JOINT_MOTOR_MAP.find(joint) != Constants::JOINT_MOTOR_MAP.end()) {
 		setMotorPos(Constants::JOINT_MOTOR_MAP.at(joint), targetPos);
-	}
-	// FIXME: need to do some extra control (probably implementing our own PID control) for the
-	// differential position, since the potentiometers are giving us joint position instead of
-	// motor position.
-	/*
-	else if (joint == jointid_t::differentialPitch || joint == jointid_t::differentialRoll) {
-		std::lock_guard<std::mutex> lk(wristPosMutex);
-		if (joint == jointid_t::differentialPitch){
-			commandedWristPos.pitch = targetPos;
-		} else {
-			commandedWristPos.roll = targetPos;
-		}
-		gearpos_t gearPos = wristKinematics().jointPosToGearPos(commandedWristPos);
-		setMotorPos(motorid_t::differentialLeft, gearPos.left);
-		setMotorPos(motorid_t::differentialRight, gearPos.right);
-	}
-	*/
-	else {
-		// FIXME: this should ideally never happen, but we don't have support for all joints
-		// yet because we don't know anything about the drill arm (and need to do extra work
-		// for the differential)
+	} else {
 		LOG_F(WARNING, "setJointPos called for currently unsupported joint %s",
 			  util::to_string(joint).c_str());
 	}
@@ -163,6 +147,19 @@ types::DataPoint<int32_t> getJointPos(robot::types::jointid_t joint) {
 		} else {
 			return {};
 		}
+	} else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) {
+		auto mdegToRad = [](int32_t mdeg) { return (mdeg / 1000.0) * (M_PI / 180.0); };
+		auto lPos = getMotorPos(motorid_t::wristDiffLeft).transform(mdegToRad);
+		auto rPos = getMotorPos(motorid_t::wristDiffRight).transform(mdegToRad);
+		if (lPos.isValid() && rPos.isValid()) {
+			kinematics::gearpos_t gearPos(lPos.getData(), rPos.getData());
+			auto jointPos = Globals::wristKinematics.gearPosToJointPos(gearPos);
+			float angle = joint == jointid_t::wristPitch ? jointPos.pitch : jointPos.roll;
+			return DataPoint<int32_t>(lPos.getTime(),
+									  static_cast<int32_t>(angle * (180.0 / M_PI) * 1000));
+		} else {
+			return {};
+		}
 	} else {
 		// This should ideally never happen, but may if we haven't implemented a joint yet.
 		LOG_F(WARNING, "getJointPos called for currently unsupported joint %s",
@@ -214,6 +211,14 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) {
 				}
 			}
 		}
+	} else if (joint == jointid_t::wristPitch || joint == jointid_t::wristRoll) {
+		setJointPowerValue(joint, power);
+		kinematics::jointpos_t jointPwr(getJointPowerValue(jointid_t::wristPitch),
+										getJointPowerValue(jointid_t::wristRoll));
+		kinematics::gearpos_t gearPwr =
+			Globals::wristKinematics.jointPowerToGearPower(jointPwr);
+		setMotorPower(motorid_t::wristDiffLeft, gearPwr.left);
+		setMotorPower(motorid_t::wristDiffRight, gearPwr.right);
 	} else {
 		LOG_F(WARNING, "setJointPower called for currently unsupported joint %s",
 			  util::to_string(joint).c_str());

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:

src/world_interface/data.h

@@ -67,7 +67,8 @@ enum class motorid_t {
 	shoulder,
 	elbow,
 	forearm,
-	wrist,
+	wristDiffRight,
+	wristDiffLeft,
 	hand,
 	activeSuspension
 };
@@ -90,7 +91,8 @@ enum class jointid_t {
 	shoulder,
 	elbow,
 	forearm,
-	wrist,
+	wristPitch,
+	wristRoll,
 	hand,
 	activeSuspension,
 	ikForward,
@@ -99,8 +101,8 @@ 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>(
 	{{"frontLeftSwerve", jointid_t::frontLeftSwerve},
@@ -111,7 +113,8 @@ constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, join
 	 {"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},

src/world_interface/noop_world.cpp

@@ -6,7 +6,6 @@
 
 // using namespace kinematics;
 using kinematics::DiffDriveKinematics;
-using kinematics::DiffWristKinematics;
 using namespace navtypes;
 using namespace robot::types;
 
@@ -16,17 +15,12 @@ extern const WorldInterface WORLD_INTERFACE = WorldInterface::noop;
 
 namespace {
 DiffDriveKinematics drive_kinematics(1); // doesn't really matter what we set this to
-DiffWristKinematics wrist_kinematics;
 } // namespace
 
 const DiffDriveKinematics& driveKinematics() {
 	return drive_kinematics;
 }
 
-const DiffWristKinematics& wristKinematics() {
-	return wrist_kinematics;
-}
-
 void world_interface_init(
 	std::optional<std::reference_wrapper<net::websocket::SingleClientWSServer>> wsServer,
 	bool initMotorsOnly) {}