The values returned for Joint 4 and Joint 5 have been "entangled" and actually just reported Motor 4 and Motor 5.
For Joint 4: ANGLE_STEPS is just MOTOR 4, not joint 4. It interacts with motors 3 and 5 to move the joint. We need to average motor 4 and motor 5, then remove the effect of joint 3. Joint 3 changes the diff by an amount equal to it's stepped position, divided by the different in gearing between joints 3 and 4. Removing that amount gives us the correct position of Joint 4
For Joint 5: ROT_STEPS is just MOTOR 5, not joint 5. It interacts with motor 4 to move the joint. We need to take the difference between motor 4 and motor 5 divided by 2. Joint 3 has no effect, as it changes both diff gears the same, and Joint 5 only reacts to the difference.
For
r 0 #StepAngleshttps://github.com/HaddingtonDynamics/Dexter/wiki/read-from-robot#keywordsThe values returned for Joint 4 and Joint 5 have been "entangled" and actually just reported Motor 4 and Motor 5.
For Joint 4: ANGLE_STEPS is just MOTOR 4, not joint 4. It interacts with motors 3 and 5 to move the joint. We need to average motor 4 and motor 5, then remove the effect of joint 3. Joint 3 changes the diff by an amount equal to it's stepped position, divided by the different in gearing between joints 3 and 4. Removing that amount gives us the correct position of Joint 4
For Joint 5: ROT_STEPS is just MOTOR 5, not joint 5. It interacts with motor 4 to move the joint. We need to take the difference between motor 4 and motor 5 divided by 2. Joint 3 has no effect, as it changes both diff gears the same, and Joint 5 only reacts to the difference.
Corrected in https://github.com/HaddingtonDynamics/Dexter/commit/d0b62e74d3b099607d012ec9ac9074febdb3f59d