working angle calculation
trying with odometer and wallfollow