 void parsePen(char * cmd) {
   char * tmp;
   strtok_r(cmd, " ", &tmp);
-  int pos = atoi(tmp);
-  servoPen.write(pos);
+  int tpos = atoi(tmp);
+
+  servoMove(tpos);
 }
 
 void parsePenPosSetup(char * cmd) {

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

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

  initRobotSetup();
  initPosition();

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

