While I am no expert in robotics or in functional programming, at a quick glance your code seems rather imperative. You are probably already familiar with this

While I am no expert in robotics or in functional programming, at a quick glance your code seems rather imperative. You are probably already familiar with this but see https://github.com/fpinscala/fpinscala https://github.com/fpinscala/fpinscala for some examples of functional programming in Scala. Functionality like ugliness, is in the eye of the beholder, particularly when dealing with multi-paradigm languages like Scala. But certain idioms are expected...