Tidied up a bit. There is a problem drawing the sensors though.