Safety fix (encoder values)+fixed sensor config

Have yet to make gyros work :P