In an effort to make less terrible code I've build a DHRobot that uses D-H parameters.  This greatly simplifies the underlying fabric of all the robot arms in the code.  I've already got better IK working in the SHSixi2 and shortly I will be upgrading the other arms to use the new system.

Next is to get JInput working so that I can drive with a joystick.