Added auto/manual robot position & heading tracking, added sample main file (command-line style)