removed most of the debugging print outs
Got rid of some print lines and added back in get zmp. Maestor is ready to rock and roll