Implemented leg commands

All leg commands on master node now implemented. Added debug for leg
feedback to serial controller.
Updated message format

Always use correlation id | command | info whether sending or receiving.