Motion logic depends on curvature instead of circle radii

This improves the algorithm because it excludes inconvenient edge cases (due to IEEE floating point spec)
Fixed bugs in the acceleration/deceleration and slow down for turns