setOffset() and setPosition() don't set heading correctly

[sfe-SparkFro]

The root problem is that poseToRegs() wraps the register value from -pi to pi:

SparkFun_Qwiic_OTOS_FTC_Java_Library/SparkFunOTOS.java

Line 788 in a04b4f5

short rawH = (short) _angularUnit.toRadians(pose.h * hToRaw);

Should be (_angularUnit.toRadians(pose.h) * hToRaw)