Minor changes and tested with hardware