  * reliable.
  */
 
-#define SPEED 200 // maximum motor speed when turning or going straight
-
-#define CALIB_SAMPLES 70
-#define CRA_REG_M_220HZ 0x1C // CRA_REG_M value for magnetometer 220 Hz update rate
-
-// allowed deviation (in degrees) relative to target angle that must be achieved before driving straight
-#define DEVIATE_THRESHOLD 5
-
-#define DEVIATE_RIGHT_THRESHOLD   DEVIATE_THRESHOLD
-#define DEVIATE_LEFT_THRESHOLD    (360 - DEVIATE_THRESHOLD)
-
-/* Public variables that may be used for compass are:
- * m_min - minimum magnetic vector
- * m_max - maximum magnetic vector
- * m     - magnetic vector
- * a     - acceleration vector
- */
+#define SPEED 200 // Maximum motor speed when turning or going straight
+
+#define CALIBRATION_SAMPLES 70  // Number of compass readings to take when calibrating
+#define CRA_REG_M_220HZ 0x1C    // CRA_REG_M value for magnetometer 220 Hz update rate
+
+// Allowed deviation (in degrees) relative to target angle that must be achieved before driving straight
+#define DEVIATION_THRESHOLD 5
+
+#define DEVIATION_RIGHT_THRESHOLD   DEVIATION_THRESHOLD
+#define DEVIATION_LEFT_THRESHOLD    (360 - DEVIATION_THRESHOLD)
 
 ZumoMotors motors;
 Pushbutton button(ZUMO_BUTTON);
 LSM303 compass;
 
-// Setup will calibrate our compass
-// We calibrate by finding maximum/minimum magnetic readings
+// Setup will calibrate our compass by finding maximum/minimum magnetic readings
 void setup()
 {
   // The highest possible magnetic value to read in any direction is 2047


void loop()
{
  int heading, relHeading;
  static int targetHeading = averageHeading();

  // LSM303::vector{1,0,0} starts our heading to point North
  // Heading is given in degrees away from North, increasing clockwise
  heading = averageHeading();

  // This gives us our relative heading in respect to the angle we want to drive in
  relHeading = relativeHeading(targetHeading, heading);

  Serial.print("Target heading: ");
  Serial.print(targetHeading);
  Serial.print("    Actual heading: ");
  Serial.print(heading);
  Serial.print("    Difference: ");
  Serial.print(relHeading);

  // If we have turned to the direction we want to be pointing, go straight and then do another turn
  if((relHeading < DEVIATE_RIGHT_THRESHOLD) || (relHeading > DEVIATE_LEFT_THRESHOLD))
  {
    motors.setSpeeds(SPEED, SPEED);


  motors.setLeftSpeed(-SPEED*adjust - 100);
}

// Converts x and y components of a vector to a heading in degrees.
// This function is used instead of LSM303::heading() because we don't
// want the acceleration of the Zumo to factor spuriously into the
// tilt compensation that LSM303::heading() performs. This calculation
// assumes that the Zumo is always level.
int heading(LSM303::vector v)
{
  float x_scaled =  2.0*(float)(v.x - compass.m_min.x) / ( compass.m_max.x - compass.m_min.x) - 1.0;
  float y_scaled =  2.0*(float)(v.y - compass.m_min.y) / (compass.m_max.y - compass.m_min.y) - 1.0;

  int angle = round(atan2(y_scaled, x_scaled)*180 / M_PI);
  if (angle < 0)
    angle += 360;
  return angle;
}

// Yields the angle difference in degrees between two headings
int relativeHeading(int heading_from, int heading_to)
{
  if(heading_from < heading_to)
  {
    return heading_to - heading_from;
  }
  else
  {
    return heading_to + (360 - heading_from);
  }
}

// Average 10 vectors to get a better measurement and help smooth out
// the motors' magnetic interference.
int averageHeading()
{
  LSM303::vector avg = {0, 0, 0};
