Added debugging output and actual hardware write to motor node.  Still untested