/* Code by Mira and Maria for Ron Alexander Webster the Third
A.K.A. The Following Thingy that instructs two motors to move
at varying speeds based on the compas reading. The robot would
move north, except in the presence of a strong magnet, where
the compass would point in its direction, and the robot follows
the compass. */

#include &lt;Wire.h&gt; 

int compassAddress = 0x32 &gt;&gt; 1;
  // From datasheet compass address is 0x32 for write operations,
  // or 0x33 for read operations.
  // shift the address 1 bit right, the Wire library only needs the 7
  // most significant bits for the address

int heading = 0;  // variable to hold the heading angle
int tilt = 0;     // variable to hold the tilt angle
int roll = 0;     // variable to hold the roll angle

byte responseBytes[6];  // for holding the sensor response bytes

  const int motor1PinLeft = 3;     // H-bridge leg 1 (pin 2, 1A)
  const int motor2PinLeft = 11;    // H-bridge leg 2 (pin 7, 2A)
  const int motor1PinRight =5;
  const int motor2PinRight=6;
  const int enablePinLeft = 9;   // H-bridge enable pins
  const int enablePinRight = 10;
  const int ledPin = 13;      // LED just to say it is on

  int motorSpeed=0;

void setup()
{
  delay(500);  //Wait 500 milli-seconds for device initialization
  Wire.begin();        // join i2c bus (address optional for master)
  Serial.begin(9600);  // start serial communication at 9600bps
  pinMode(13, OUTPUT);  //set arduino onboard LED pin
  digitalWrite(13, HIGH);  // just turn ON the onboard LED 

    // set all the HBridge pins as outputs:
    pinMode(motor1PinLeft, OUTPUT);
    pinMode(motor2PinLeft, OUTPUT);
    pinMode(motor1PinRight, OUTPUT);
    pinMode(motor2PinRight, OUTPUT); 

    pinMode(enablePinLeft, OUTPUT);
    pinMode(enablePinRight, OUTPUT);

    // set enablePins high so both motors can turn on:
    digitalWrite(enablePinLeft, HIGH);
    digitalWrite(enablePinRight, HIGH); 

} 

void loop()
{
  readSensor();  // read data from the HMC6343 sensor
  // Note that heading, tilt and roll values are in
  // tenths of a degree, for example
  // if the value of heading is 1234 would mean 123.4
  // degrees, that's why the result
  // is divided by 10 when printing.
  Serial.print("Heading: ");
  Serial.print(heading / 10, DEC);
  Serial.print(" Tilt: ");
  Serial.print(tilt / 10, DEC);
  Serial.print(" Roll: ");
  Serial.println(roll / 10, DEC);
  delay(200);                   // wait for half a second 

 // GO FORWARD::    

motorspeed=250;

    if(heading/10&gt;340 || heading/10&lt;20){
    digitalWrite(enablePinLeft, HIGH);
    digitalWrite(enablePinRight, HIGH); 

//left motor go forward:
    analogWrite(motor1PinLeft, motorSpeed);
    analogWrite(motor2PinLeft, 0);

//right motor go forward
    analogWrite(motor1PinRight, motorSpeed);
    analogWrite(motor2PinRight, 0);
    }

 //TURN RIGHT:

    if(heading/10&gt;20 &amp;&amp; heading/10&lt;160){        

    digitalWrite(enablePinLeft, HIGH);
    digitalWrite(enablePinRight, HIGH); 

//map the motor speed so that the more of a right
//angle that nees to be made, the faster the
//motors will turn:

motorSpeed=map(heading/10,340, 200,0,250);    

//left motor go forward to turn right at motorspeed
    analogWrite(motor1PinLeft, motorSpeed);
    analogWrite(motor2PinLeft, 0);

//right motor go backward to turn right
    analogWrite(motor1PinRight, 0);
    analogWrite(motor2PinRight, motorSpeed/2);

    } 

//TURN LEFT: 

    if(heading/10&lt;340 &amp;&amp; heading/10&gt;200){       

    digitalWrite(enablePinLeft, HIGH);
    digitalWrite(enablePinRight, HIGH); 

//map the motor speed so that the more of a right
//angle that nees to be made, the faster the
//motors will turn:

motorSpeed=map(heading/10, 20,160,0,250);

//left motor go backward to turn left:
    analogWrite(motor1PinLeft, 0);
    analogWrite(motor2PinLeft, motorSpeed);

 //right motor go backward to turn left:
    analogWrite(motor1PinRight, motorSpeed/2);
    analogWrite(motor2PinRight, 0);
    }

//  STOP if the magnet is directly behind the compass::

    if(heading/10&gt;160 &amp;&amp; heading/10&lt;200){    

        digitalWrite(enablePinLeft, LOW);
    digitalWrite(enablePinRight, LOW); 

    }
}

void readSensor()
{
  // step 1: instruct sensor to read echoes :::::

  Wire.beginTransmission(compassAddress);
  // transmit to device
  // the address specified in the datasheet is 66 (0x42)
  // but i2c adressing uses the high 7 bits so it's 33 

   // Send a "Post Heading Data"(0x50) command to the HMC6343:
   Wire.send(0x50);         

  Wire.endTransmission();  // stop transmitting 

  // step 2: wait for readings to happen :::::

  delay(2);// datasheet suggests at least 1 ms 

  // step 3: request reading from sensor :::::

  // request 6 bytes from slave device #33:
  Wire.requestFrom(compassAddress, 6); 

  // step 4: receive reading from sensor ::::

  if(6 &lt;= Wire.available())  // if six bytes were received
  {
    for(int i = 0; i&lt;6; i++) {
      responseBytes[i] = Wire.receive();
    }
  }

  // heading MSB and LSB
  heading = ((int)responseBytes[0]&lt;&lt;8) | ((int)responseBytes[1]);
  // tilt MSB and LSB
  tilt = (((int)responseBytes[2]&lt;&lt;8) | ((int)responseBytes[3]));
  // roll MSB and LSB
  roll = (((int)responseBytes[4]&lt;&lt;8) | ((int)responseBytes[5]));
}</div></pre>