spineless

What exactly is the issue?  I see your code does a bunch of stuff.  
 
1. Try just looking at your code and dont include input from the hardware.   Load the registers in software to isolate the inputs....Does that prove the rest of the code is working?
 
2. Try isolating the output by only getting your inputs then displaying the register values on the monitor. 
 
I am unclear as to what the issue is.  Can you break it down into 5 or 10 lines of code?
 
Spineless

What exactly is the issue?  I see your code does a bunch of stuff.  
 
1. Try just looking at your code and dont include input from the hardware.   Load the registers in software to isolate the inputs....Does that prove the rest of the code is working?
 
2. Try isolating the output by only getting your inputs then displaying the register values on the monitor. 
 
I am unclear as to what the issue is.  Can you break it down into 5 or 10 lines of code?
 
Spineless

You may be overrunning your serial buffer. To test this, insert some 100-200ms delays between the serial.print statements and see of the output looks more like you expect.  If that fixes things you can reduce the delays experimentally until you start seeing errors in the output. This will  help determine the smallest delays that will work.