   initRobotSetup();
   initPosition();
 
-  servoPen.write(roboSetup.data.penUpPos);
+  servoMove(roboSetup.data.penUpPos);
   delay(100);
   servoPen.attach(servopin);
 

int xlimit_pin2 = A3;
int servopin = A1;
Servo servoPen;

float curX, curY, curZ;
float tarX, tarY, tarZ; // target xyz position

void parsePen(char * cmd) {
  char * tmp;
  strtok_r(cmd, " ", &tmp);
  int tpos = atoi(tmp);

  servoMove(tpos);
}

void parsePenPosSetup(char * cmd) {
