Known working init and test tower code
Merge branch 'develop' of github.com:filitchp/spiritedrobotics into develop