Updated MCPM functionality

Added correct math for vertical axis in MCPM::gotoLocation.
added pull down resistors to vertical axis done.
added debug lines to goto.