Added beginnings of test wrapper.
Merge branch 'Cammode' of https://github.com/OlinRoboticsAndBioinspiration/dynaroach into Cammode
Added wrapper.