Correct register addresses and servo pmw out pin