Added new Sensor test

With help of libs from I2Cdev the sensor now works flawlessly.