now correctly accelerating and decelerating
overestimate radius for security while debugging on robot
print some debug stuff