added CPP version of Motors class
Replaced python nav, pilot and behaviour with c++ versions
merged with previous changes