Update functions to enforce hierarchy
Resolves issue #25
Creates DesiredMotorVals struct to be passed from function to function
Fix bug where memset was used incorrectly
memset can only initialize single byte values