Adding new sensor testing code

For the LEGO protobot