 
     if (y == 0) return;
 
-    float cx = x - state.x;
-    float cy = y - state.y;
+    float cx1 = x - state.x;
+    float cy1 = y - state.y;
 
     //v.rotate(degToRad(-this.state.angle));
-    float ang = -state.ang * PI / 180;
-    cx = cx * cos(ang) - cy * sin(ang);
-    cy = cx * sin(ang) + cy * cos(ang);
+    float ang = -state.ang * DEGTORAD;
+    float ca = cos(ang);
+    float sa = sin(ang);
+    float cx = cx1 * ca - cy1 * sa;
+    float cy = cx1 * sa + cy1 * ca;
 
     float m = -cx / cy;
 


static void driveTo(float x, float y) {
  // calc angle
  float ang = atan2(y-state.y, x-state.x) * 180 / PI;
  // now angle delta
  ang = ang - state.ang;
  if (ang > 180)

    float cl, cr;

    if (x1 < 0) {
        targetAng = atan2(cy, -x1 + cx) * RADTODEG;

        cl = 2 * PI * (-WHEELSPACING/2 - x1);
        dl = cl * targetAng/360;
