Debug odometry, added state in state machine