Totts can follow a leader, will implement possibility to follow group if they are close, otherwise follow other object

Signed-off-by: Herman Rödström <herman@rodstrom.se>
Small fixes, awaiting merge to solve initial waypoint faulty position.

Signed-off-by: Herman Rödström <herman@rodstrom.se>