I didn't look at the code, because i'm at work, but you should post your schematic too!