Add ISR Timer logic, clean up galvo home