   }
 }
 
-void driveForward(long distance)
-{
+void driveTo(long x, long y) {
+  Serial.print(x);
+  Serial.print(",");
+  Serial.println(y); 
+  
+  // calc angle
+  long ang = atan2(y-position.y, x-position.x) * 180 / PI;
+  // now angle delta
+  ang = ang - position.ang;
+  if (ang > 0) 
+    pushCmd("LT " + String(ang));
+  else 
+    pushCmd("RT " + String(-ang));
+  
+  // and distance
+  long dist = sqrt(sqr(y-position.y) + sqr(x-position.x));
+  pushCmd("FD "+String(dist));
+}
+
+void drive(long distance)
+{ 
+  // update state
+  position.x += distance * cos(position.ang * PI / 180);
+  position.y += distance * sin(position.ang * PI / 180);
+  
+  Serial.print("Pos: ");
+  Serial.print(position.x);
+  Serial.print(",");
+  Serial.println(position.y);
+  
+  distance *= STEPS_PER_MM;
   stepperL.move(distance);
   stepperR.move(distance);
 }
 
-void driveBackward(long distance)
-{
-  stepperL.move(-distance);
-  stepperR.move(-distance);
-}
-
-void leftTurn(long distance)
-{
-  stepperR.move(distance);
-  stepperL.move(-distance);
-}
-
-void rightTurn(long distance)
-{
-  stepperR.move(-distance);
-  stepperL.move(distance);
+void turn(long ang)
+{ 
+  // update state
+  position.ang += ang;
+  
+  ang *= STEPS_PER_DEG;
+  stepperR.move(ang);
+  stepperL.move(-ang);
+ 
+  
+  // correct wrap around
+  if (position.ang > 360) position.ang -= 360;
+  if (position.ang < -360) position.ang += 360;
+  
+  Serial.print("Angle: ");
+  Serial.println(position.ang);
 }
 
 void buzz(int len)

AccelStepper stepperL(AccelStepper::HALF4WIRE, motorLPin1, motorLPin3, motorLPin2, motorLPin4);
AccelStepper stepperR(AccelStepper::HALF4WIRE, motorRPin1, motorRPin3, motorRPin2, motorRPin4);

Servo penliftServo;

long buzzEnd = 0;


void setup()

  dlcs("FD 2000");
  
}

long sqr(long v) {
  return v*v;
}
