On second thought, I've been meaning to cleanup the code for this device. I think now is a good time.
When I wrote it, it was the first assembler I've ever done. I learned a lot from that and wrote the servo controller using much better technique including all interrupt driven code. The tester, in my opinion, is poorly written and needs to be addressed (re-written).
So you'll have to give me some time to tidy things up. Then it will be a breeze to add new features like this as we go forward.
Hang tight,
Dave