Adding motor test, cleaning up #defines, adding mode flag, cleaning up scanning procedure.