Fixed bug in calculating the time it takes to do motion in the case where the drive cannot reach the maximum velocity.