works now but with an annoying line from 0,0 to robot position :P