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

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

void parsePenPosSetup(char * cmd) {

void initPosition() { curX = 0; curY = 0; posA = 0; posB = 0; }


// ----------------------------------------
// SERVO

void servoMove(int tpos) {
  int d = tpos - servo_position;

  for (int i = 0; servo_position != tpos; i++) {
    servo_position += (d > 0) ? 1 : -1;
    servoPen.write(servo_position);
    delay(15);
  }
}

// ----------------------------------------
// PARSING
