Some test code that was working.

Currently may not be working, just some
syntax stuff to fix to continue servo testing.