More code reorganization/merging. Works with 5 @ 500hz.