Wenn die Hardware funktioniert, wirds wohl an der Software liegen...
Da hier keiner hellsehen kann ein kleiner Tipp am Rande: mit gepostetem Code lässt es sich leichter einen Fehler darin zu suchen