add heading method, needs debugging

Signed-off-by: Koen Kooi <koen@dominion.thruhere.net>
Add move to raise robot on boot

Signed-off-by: Koen Kooi <koen@dominion.thruhere.net>