Modified memory addressing in driver because cpp program refers to joints as 0...5 instead of 1...6
All functionality for talking to hardware has been added to cpp, but is currently commented out