Adding LED test function and fixing bug