Add encoder and new accelerometer test code

Tested works