Fixed bug with robot displacement only being incremented once
Fixed bug with all displacements being 0