Update firmware.ino

Updates in optimization and error fixes