Fixing issue with AVR code