Updated the code to reflect the final motor command outputs. Outputs motor command 1 and motor command 2. Final test will be needed for teh serial write