A picture of the stair-stepping might make it more clear what is happening. Since all you changed is the code, it does not make sense that it would be Mach 3. Try this, mount something to your table that you can indicate against. Set your machine zero to that location. Run one part positioned on the table so that the table does not come back into your indicator. then re-zero the machine to see if it is still reading zero. Repeat for each axis. This will let you know if you are loosing steps or if the G-code is screwy.