C 2 Schrittmotoren ansteuern (mit Arduino) - was mache ich falsch?

[Jawaiica]

Hallo Leute,
ich bastel grade an einem Prototypen meiner eigenen CNC Maschine rum und habe jetzt schon einiges geschafft. Von der Präzision her ist er besser als erwartet, ich habe momentan aber nur 2 Achsen und einen Bleistift als 3. feste Achse^^. Gerade Linien (Horizontal, Vertikal und diagonale Linien) kann ich mit meiner Steuerung schon problemlos zeichnen, allerdings keine beliebigen Linien geschweige denn Kreise.
Angesteuert werden die Motoren mit einem Arduino 2560 Board, aber mir geht es jetzt nicht um einen Code, sondern um ein Verständnis Problem, dennoch wäre es nett wenn ihr euch den Code(von der logischen Seite) mal angucken könntet.
Um solche beliebigen Linien zu zeichnen, verwende ich den Bresenham Algorithmus und dazu habe ich auch schon zwei Seiten gefunden, allerdings weiß ich immer noch nicht, wieso es bei mir nicht funktioniert.
Naja hier erst einmal "mein" Code (kopiert und abgeändert von http://rosettacode.org/wiki/Bitmap/Bresenham's_line_algorithm#C um die Steuerung zu testen, da meine Versuche gescheitert waren... ).

#include <Stepper.h>

//Anzahl der Steps für eine volle Umdrehung
#define STEPS 200

//Instanz für Schrittmotoren
Stepper XStepper(STEPS, 8, 9, 10, 11);
Stepper YStepper(STEPS, 2, 3, 4, 5);

void setup()
{
  Serial.begin(9600);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
}

void drawLine(int x0, int y0, int x1, int y1) {
  int dx = abs(x1-x0), sx = x0<x1 ? 1 : -1;
  int dy = abs(y1-y0), sy = y0<y1 ? 1 : -1; 
  int err = (dx>dy ? dx : -dy)/2, e2 = 0;
  int xtemp = 0, ytemp = 0;
  
  XStepper.setSpeed(30);
  YStepper.setSpeed(30);
 
  digitalWrite(7, HIGH);
  digitalWrite(6, HIGH);
  for(;;){
    
    if (sx > 0) { XStepper.step((x0-xtemp)); delay(2); Serial.print("X:"); Serial.print((x0-xtemp)); Serial.println(""); }
    if (sx < 0) { XStepper.step((x0-xtemp)); delay(2); Serial.print("X:"); Serial.print((x0-xtemp)); Serial.println(""); }
    if (sy > 0) { YStepper.step((y0-ytemp)); delay(2); Serial.print("Y:"); Serial.print((y0-ytemp)); Serial.println(""); }
    if (sy < 0) { YStepper.step((y0-ytemp)); delay(2); Serial.print("Y:"); Serial.print((y0-ytemp)); Serial.println(""); }
    
    xtemp = x0;
    ytemp = y0;
    
    if (x0==x1 && y0==y1) break;
    e2 = err;
    if (e2 >-dx)     { err -= dy; x0 += sx; }
    if (e2 < dy)     { err += dx; y0 += sy; }
  }
  digitalWrite(7, LOW);
  digitalWrite(6, LOW);
}

void loop()
{
  drawLine(0,0,2500,7500);
  delay(50000);
}

Sobald ich alles auf das Board geladen habe und über den "Seriellen Monitor" gucke, was die Motoren machen sollen, sehe ich, dass alles perfekt laufen müsste, allerdings tut es das nicht in der Praxis... Ich kriege entweder zu kurze oder nur diagonale Linien hin. Wisst ihr eventuell einen Rat?

MfG Jawaiica