   initPosition();
 }
 
-void initPosition() {
-  curX = 0;
-  curY = 0;
-  posA = 0;
-  posB = 0;
-}
-
-/************** calculate movements ******************/
-void parseCoordinate(char * cmd)
-{
+void initPosition() { curX = 0; curY = 0; posA = 0; posB = 0; }
+
+
+
+// ----------------------------------------
+// PARSING
+
+void parseCoordinate(char * cmd) {
   char * tmp;
   char * str;
   str = strtok_r(cmd, " ", &tmp);
   tarX = curX;
   tarY = curY;
-  while(str!=NULL){
+  while (str!=NULL) {
     str = strtok_r(0, " ", &tmp);
-    if(str[0]=='X'){
-      tarX = atof(str+1);
-    }else if(str[0]=='Y'){
-      tarY = atof(str+1);
-    }else if(str[0]=='Z'){
-      tarZ = atof(str+1);
-    }else if(str[0]=='A'){
-      stepAuxDelay = atoi(str+1);
-    }
+    if (str[0] == 'X') tarX = atof(str + 1);
+    else if (str[0] == 'Y') tarY = atof(str + 1);
+    else if (str[0] == 'Z') tarZ = atof(str + 1);
+    else if (str[0] == 'A') stepAuxDelay = atoi(str + 1);
   }
   prepareMove();
 }
 
-void echoRobotSetup()
-{
-  Serial.print("M10 XY ");
-  Serial.print(roboSetup.data.width);Serial.print(' ');
-  Serial.print(roboSetup.data.height);Serial.print(' ');
-  Serial.print(curX);Serial.print(' ');
-  Serial.print(curY);Serial.print(' ');
-  Serial.print("A");Serial.print((int)roboSetup.data.motoADir);
-  Serial.print(" B");Serial.print((int)roboSetup.data.motoBDir);
-  Serial.print(" H");Serial.print((int)roboSetup.data.motorSwitch);
-  Serial.print(" S");Serial.print((int)roboSetup.data.speed);
-  Serial.print(" U");Serial.print((int)roboSetup.data.penUpPos);
-  Serial.print(" D");Serial.println((int)roboSetup.data.penDownPos);
-}
-
-void echoEndStop()
-{
-  Serial.print("M11 ");
-  Serial.print(digitalRead(xlimit_pin1)); Serial.print(" ");
-  Serial.print(digitalRead(xlimit_pin2)); Serial.print(" ");
-  Serial.print(digitalRead(ylimit_pin1)); Serial.print(" ");
-  Serial.println(digitalRead(ylimit_pin2));
-}
-
-void syncRobotSetup()
-{
-  int i;
-  for(i=0;i<64;i++){
-    EEPROM.write(i,roboSetup.buf[i]);
-  }
-}
-
-void parseRobotSetup(char * cmd)
-{
+void parseRobotSetup(char * cmd) {
   char * tmp;
   char * str;
   str = strtok_r(cmd, " ", &tmp);
   while(str!=NULL){
     str = strtok_r(0, " ", &tmp);
-    if(str[0]=='A'){
-      roboSetup.data.motoADir = atoi(str+1);
-    }else if(str[0]=='B'){
-      roboSetup.data.motoBDir = atoi(str+1);
-    }else if(str[0]=='H'){
-      roboSetup.data.height = atoi(str+1);
-    }else if(str[0]=='W'){
-      roboSetup.data.width = atoi(str+1);
-    }else if(str[0]=='S'){
-      roboSetup.data.speed = atoi(str+1);
+    if (str[0] == 'A') roboSetup.data.motoADir = atoi(str + 1);
+    else if (str[0] == 'B') roboSetup.data.motoBDir = atoi(str + 1);
+    else if (str[0] == 'H') roboSetup.data.height = atoi(str + 1);
+    else if (str[0] == 'W') roboSetup.data.width = atoi(str + 1);
+    else if (str[0] == 'S'){
+      roboSetup.data.speed = atoi(str + 1);
+      // stepdelay_min = atoi(str + 1);
     }
   }
   syncRobotSetup();
 }
 
-void parseAuxDelay(char * cmd)
-{
+void parseAuxDelay(char * cmd) {
   char * tmp;
   strtok_r(cmd, " ", &tmp);
   stepAuxDelay = atoi(tmp);
 }
 
-void parsePen(char * cmd)
-{
+void parsePen(char * cmd) {
   char * tmp;
   strtok_r(cmd, " ", &tmp);
   int pos = atoi(tmp);
   servoPen.write(pos);
 }
 
-void parsePenPosSetup(char * cmd)
-{
+void parsePenPosSetup(char * cmd) {
   char * tmp;
   char * str;
   str = strtok_r(cmd, " ", &tmp);
   while(str!=NULL){
     str = strtok_r(0, " ", &tmp);
-    if(str[0]=='U'){
-      roboSetup.data.penUpPos = atoi(str+1);
-    }else if(str[0]=='D'){
-      roboSetup.data.penDownPos = atoi(str+1);
-    }
+    if (str[0] == 'U') roboSetup.data.penUpPos = atoi(str + 1);
+    else if (str[0] == 'D') roboSetup.data.penDownPos = atoi(str + 1);
   }
   syncRobotSetup();
 }
 
-void parseMcode(char * cmd)
-{
-  int code;
-  code = atoi(cmd);
+void parseMcode(char * cmd) {
+  int code = atoi(cmd);
   switch(code){
-    case 1:
-      parsePen(cmd);
-      break;
-    case 2:
-      parsePenPosSetup(cmd);
-      break;
-    case 3:
-      parseAuxDelay(cmd);
-      break;
-    case 5:
-      parseRobotSetup(cmd);
-      break;
-    case 10:
-      echoRobotSetup();
-      break;
-    case 11:
-      echoEndStop();
-      break;
-  }
-}
-
-void parseGcode(char * cmd)
-{
-  int code;
-  code = atoi(cmd);
+    case 1:   parsePen(cmd); break;
+    case 2:   parsePenPosSetup(cmd); break;
+    case 3:   parseAuxDelay(cmd); break;
+    case 5:   parseRobotSetup(cmd); break;
+    case 10:  echoRobotSetup(); break;
+    case 11:  echoEndStop(); break;
+  }
+}
+
+void parseGcode(char * cmd) {
+  int code = atoi(cmd);
   switch(code){
     case 0:
-    case 1:
-      parseCoordinate(cmd);
-      break;
-    case 28:
-      tarX=0; tarY=0;
-      goHome();
-      break;
+    case 1: parseCoordinate(cmd); break;
+    case 28: tarX=0; tarY=0; goHome(); break;
   }
 }
 
 void parseCmd(char * cmd) {
   Serial.println("Command received.");
-  if (cmd[0]=='G') parseGcode(cmd+1);
-  else if (cmd[0]=='M') parseMcode(cmd+1);
-  else if (cmd[0]=='P') {
-    Serial.print("POS X");
-    Serial.print(curX);
-    Serial.print(" Y");
-    Serial.println(curY);
-  }
-}
-
-// local data
-void initRobotSetup()
-{
-  int i;
-  for(i=0;i<64;i++){
-    roboSetup.buf[i] = EEPROM.read(i);
-  }
-  if(strncmp(roboSetup.data.name,"XY4",3)!=0){
+  if (cmd[0] == 'G') parseGcode(cmd+1);
+  else if (cmd[0] == 'M') parseMcode(cmd+1);
+  else if (cmd[0] == 'P') echoPosition();
+}
+
+
+
+// ----------------------------------------
+// LOCAL DATA
+
+void initRobotSetup() {
+  for(int i = 0; i < 64; i++) roboSetup.buf[i] = EEPROM.read(i);
+
+  if (strncmp(roboSetup.data.name,"XY4", 3) != 0) {
     Serial.println("set to default setup");
     // set to default setup
-    memset(roboSetup.buf,0,64);
-    memcpy(roboSetup.data.name,"XY4",3);
+    memset(roboSetup.buf, 0, 64);
+    memcpy(roboSetup.data.name, "XY4", 3);
     roboSetup.data.motoADir = 0;
     roboSetup.data.motoBDir = 0;
     roboSetup.data.width = WIDTH;

    char buf[64];
}roboSetup;

float curX,curY,curZ;
float tarX,tarY,tarZ; // target xyz position
int tarA,tarB,posA,posB; // target stepper position
int8_t motorAfw,motorAbk;
int8_t motorBfw,motorBbk;

int ylimit_pin1 = 12;
int ylimit_pin2 = 13;

    roboSetup.data.penDownPos = 90;
    syncRobotSetup();
  }

  // init motor direction
  // yzj, match to standard connection of xy
  // A = x, B = y
  if (roboSetup.data.motoADir == 0) {
    motorAfw = -1;
    motorAbk = 1;
  } else {
    motorAfw = 1;
    motorAbk = -1;
  }
  if (roboSetup.data.motoBDir == 0) {
    motorBfw = -1;
    motorBbk = 1;
  } else {
    motorBfw = 1;
    motorBbk = -1;
  }
}

void syncRobotSetup() {
  for (int i = 0; i < 64; i++) EEPROM.write(i, roboSetup.buf[i]);
}


// ----------------------------------------
// ARDUINO

char buf[64];
int8_t bufindex;
boolean isAsleep = false;

void setup() {
  pinMode(11, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(ylimit_pin1, INPUT_PULLUP);
  pinMode(ylimit_pin2, INPUT_PULLUP);
  pinMode(xlimit_pin1, INPUT_PULLUP);
  pinMode(xlimit_pin2, INPUT_PULLUP);
  Serial.begin(115200);
  initRobotSetup();
  initPosition();

  servoPen.write(roboSetup.data.penUpPos);
  delay(100);
  servoPen.attach(servopin);

  goHome();
  Serial.println("ready");
}

void loop() {
  if (Serial.available()) {
    if (isAsleep) wakeUp();

    char c = Serial.read();
    buf[bufindex++] = c;
    if (c == '\n') {
      buf[bufindex] = '\0';
      parseCmd(buf);
      memset(buf, 0, 64);
      bufindex = 0;
    }
    if(bufindex >= 64) bufindex=0;
  } else if (!isAsleep) {
    sleep();
  }
}



// ----------------------------------------
// SERVO SLEEP

int sleep_timer = 0;

void sleep() {
  sleep_timer++;
  delay(100);
  if (sleep_timer > MS_BEFORE_SERVO_SLEEP) {
    isAsleep = true;
    servoPen.detach();
    Serial.print('good night !');
  }
}

void wakeUp() {
  sleep_timer = 0;
  isAsleep = false;
  servoPen.attach(servopin);
}




// ----------------------------------------
// ECHO

void echoPosition() {
  Serial.print("POS X");
  Serial.print(curX);
  Serial.print(" Y");
  Serial.println(curY);
}

void echoRobotSetup() {
  Serial.print("M10 XY ");
  Serial.print(roboSetup.data.width); Serial.print(' ');
  Serial.print(roboSetup.data.height); Serial.print(' ');
  Serial.print(curX); Serial.print(' ');
  Serial.print(curY); Serial.print(' ');
  Serial.print("A"); Serial.print((int)roboSetup.data.motoADir);
  Serial.print(" B"); Serial.print((int)roboSetup.data.motoBDir);
  Serial.print(" H"); Serial.print((int)roboSetup.data.motorSwitch);
  Serial.print(" S"); Serial.print((int)roboSetup.data.speed);
  Serial.print(" U"); Serial.print((int)roboSetup.data.penUpPos);
  Serial.print(" D"); Serial.println((int)roboSetup.data.penDownPos);
}

void echoEndStop() {
  Serial.print("M11 ");
  Serial.print(digitalRead(xlimit_pin1)); Serial.print(" ");
  Serial.print(digitalRead(xlimit_pin2)); Serial.print(" ");
  Serial.print(digitalRead(ylimit_pin1)); Serial.print(" ");
  Serial.println(digitalRead(ylimit_pin2));
}