 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

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

void parsePenPosSetup(char * cmd) {

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

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