Played with tuning constants and added more test functionality to main ino file. have yet to change stop conditions