Added working Z grid following code to Robot.cpp