 #include <AccelStepper.h>
 #include <Servo.h>
 
+// Maths stuff
+#define DEGTORAD  PI/180
+#define RADTODEG  180/PI
+
 // Pin definitions
-
 // Left motor driver
 #define motorLPin1  2     // IN1 on the ULN2003 driver 1
 #define motorLPin2  3     // IN2 on the ULN2003 driver 1

    float cl, cr;

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

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


    if (y == 0) return;

    float cx1 = x - state.x;
    float cy1 = y - state.y;

    //v.rotate(degToRad(-this.state.angle));
    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;

