Keypad is now fully working

Range finder needs tidied up along with code to control motors