Added code to test sensors + minor mods

Mostly me messing about with code to try to get stuff working