Ran all test cases again (no hardware), fixed a few issues. ...

Need to still run this up on the hardware to test properly

Moved test harnesses into separate _test.py modules