add encoder frequency
using constants from robot.h
fix issues
Merge pull request #14 from tobiasandersson/master

fix issues