Less hardcode, More sensor use, Not tested