 #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

  state.ang += ang;

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

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

void drive(float distance)
{
  // update state
  state.x += distance * cos(state.ang * DEGTORAD);
  state.y += distance * sin(state.ang * DEGTORAD);

  // prime the move
  int steps = distance * STEPS_PER_MM;
