 #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;

  state.ang += ang;

  // correct wrap around
  if (state.ang > 180) state.ang -= 360;
  if (state.ang < -180) state.ang += 360;

  // prime the move
  int steps = ang * STEPS_PER_DEG;
