Change to mostly using the memory mapped IO register. TESTED with boarduino, working.