Added a helper function + better debug output.
TODO: Perform state clamping on the MESSAGE, not the state.  Add function to check that every byte of the message is a reasonable value.
TODO: Revisit shoulder rotation calculation - it is probably wrong.