This is not my project but I thought that as with all this guys projects it is clear and well presented and some of you might be interested.
Edit- the guys code was in three separate files and consequently would not compile. I have combined them into one file which I think is correct but as I do not have all the hardware yet I cannot be certain