Drive straight

Added testDistPerCM() and goStraightCM().

NB: Changed the parameters of goStraight().