Fix bug with premature step end. Refactored _delay_ms() and square() for better portability.

- Fixed a premature step end bug dating back to Simen's 0.7b edge
version is fixed, from which this code is forked from. Caused by Timer2
constantly overflowing calling the Step Reset Interrupt every 128usec.
Now Timer2 is always disabled after a step end and should free up some
cycles for the main program. Could be more than one way to fix this
problem. I'm open to suggestions.

- _delay_ms() refactored to accept only constants to comply with
current compilers. square() removed since not available with some
compilers.

main.c

@@ -39,9 +39,9 @@ system_t sys;
 int main(void)
 {
   // Initialize system
-  sei(); // Enable interrupts
   serial_init(BAUD_RATE); // Setup serial baud rate and interrupts
   st_init(); // Setup stepper pins and interrupt timers
+  sei(); // Enable interrupts
 
   memset(&sys, 0, sizeof(sys));  // Clear all system variables
   sys.abort = true;   // Set abort to complete initialization