Electronic compass part 2

This is part 2 of the electronic compass with the video and the Arduino sketch.

Video:

Sketch:

/*****************************************
 * Compass with Pollin HDMM01-compass-module
 * and Nokia 5110-display
 * Last edit: 3.1.2013
 * By Markus Ulsass
 ******************************************/

/*******************************************************************************
 * This is an example sketch for our Monochrome Nokia 5110 LCD Displays
 *
 * Pick one up today in the adafruit shop!
 * ------> http://www.adafruit.com/products/338
 *
 * These displays use SPI to communicate, 4 or 5 pins are required to
 * interface
 *
 * Adafruit invests time and resources providing this open source code,
 * please support Adafruit and open-source hardware by purchasing
 * products from Adafruit!
 *
 * Written by Limor Fried/Ladyada  for Adafruit Industries.
 * BSD license, check license.txt for more information
 * All text above, and the splash screen must be included in any redistribution
 *******************************************************************************/

#include <Adafruit_GFX.h>
#include <Adafruit_PCD8544.h>

#include <Wire.h>

#define  I2ADDR       0x30
#define  TakeMeasure  0x01
#define SET_COIL 0x02 // set magnetic coil
#define RESET_COIL 0x04 // reset magnetic coil

// pin 7 - Serial clock out (SCLK)
// pin 6 - Serial data out (DIN)
// pin 5 - Data/Command select (D/C)
// pin 4 - LCD chip select (CS)
// pin 3 - LCD reset (RST)
Adafruit_PCD8544 display = Adafruit_PCD8544(7, 6, 5, 4, 3);

int r = 24;  // radius of compass rose
int x0= 60;  // x-origin
int y0 = 24; // y-origin

void setup()   {

  // Serial.begin(9600);

  display.begin();
  display.setContrast(60);
  display.clearDisplay();

  Wire.begin();

}

void loop() {

  ResetSetCoils();
 
  byte MsbX,LsbX,MsbY,LsbY;
  int x,y;
  char line[80];
  Wire.beginTransmission(I2ADDR); // Pollin HDMM01 address
  Wire.write(0x00);  
  Wire.write(TakeMeasure);
  Wire.endTransmission();
  delay(20);
  Wire.beginTransmission(I2ADDR);
  Wire.write(0x01);
  Wire.requestFrom(I2ADDR, 4);

  while(Wire.available()<4);
  MsbX  =Wire.read();          // upper  4 Bit X
  LsbX  =Wire.read();          // lower 8 Bit X
  MsbY  =Wire.read();          // upper 4 Bit Y
  LsbY  =Wire.read();          // lower 8 Bit Y
  Wire.endTransmission();         // stop transmitting
  x=((MsbX&0x0f)*256)+(LsbX);
  y=((MsbY&0x0f)*256)+(LsbY);
  x = map(x, 1960, 2151, -180, 180);
  y = map(y, 1939, 2131, -180, 180);
  double mygrad = atan2(-x, y)*180/3.1415927410;
  if (mygrad < 0)    mygrad = mygrad +360;
  int angleActual = mygrad;
  // Serial.println(mygrad); // debug
  delay(200);

  // Calculation of compass needle on lcd pointing to the direction
 
  if (angleActual >= 0 && angleActual <=45) {
    // 0-45 degrees
    int angle = angleActual/2;
    display.drawLine(x0, y0, x0+angle, y0-r, BLACK);
    // 45 degrees
    // display.drawLine(x0, y0, x0+r, y0-r, BLACK);
  }
  else if (angleActual >45 && angleActual <=90) {
    // 46-90 degrees
    int angle = (angleActual-44)/2 ;
    display.drawLine(x0, y0, x0+r, (y0-r)+angle, BLACK);
    // 90 degrees
    // display.drawLine(x0, y0, x0+r, y0, BLACK);
  }
  else if (angleActual >90 && angleActual <=135) {
    // 91-135 degrees
    int angle = (angleActual-90)/2;
    display.drawLine(x0, y0, x0+r, y0+angle, BLACK);
    // 135 degrees
    // display.drawLine(x0, y0, x0+r, y0+r, BLACK);
  }
  else if (angleActual >135 && angleActual <=180) {
    // 136-180 degrees
    int angle = (angleActual-134)/2;
    display.drawLine(x0, y0, (x0+r)-angle, y0+r, BLACK);
    // 180 degrees
    // display.drawLine(x0, y0, x0, y0+r, BLACK);
  }
  else if (angleActual >180 && angleActual <=225) {
    // 181-225 degrees
    int angle = (angleActual-178)/2;
    display.drawLine(x0, y0, x0-angle, y0+r, BLACK);
    // 225 degrees
    // display.drawLine(x0, y0, x0-r, y0+r, BLACK);
  }
  else if (angleActual >225 && angleActual <=270) {
    // 226-270 degrees
    int angle = (angleActual-222)/2;
    display.drawLine(x0, y0, x0-r, (y0+r)-angle, BLACK);
    // 270 degrees
    // display.drawLine(x0, y0, x0-r, y0, BLACK);
  }
  else if (angleActual >270 && angleActual <=315) {
    // 271-315 degrees
    int angle = (angleActual-270)/2;
    display.drawLine(x0, y0, x0-r, y0-angle, BLACK);
    // 315 degrees
    // display.drawLine(x0, y0, x0-r, y0-r, BLACK);
  }
  else if (angleActual >315 && angleActual <=360) {
    // 316-360 degrees
    int angle = (angleActual-312)/2;
    display.drawLine(x0, y0, (x0-r)+angle, y0-r, BLACK);
    // 360/ 0 degrees
    // display.drawLine(x0, y0, x0, y0-r, BLACK);
  }

  // text display

  // display.drawCircle(x0, y0, r-1, BLACK);
  // Display actual heading
  display.setTextSize(2);
  display.setTextColor(BLACK);
  display.setCursor(0,0);
  display.println(angleActual);
 
  display.setTextSize(1);
  display.setTextColor(BLACK);
  display.setCursor(x0-2,0);
  display.println("N");
 
  display.setTextSize(1);
  display.setTextColor(BLACK);
  display.setCursor((x0+r)-5,y0-3);
  display.println("0");
 
  display.setTextSize(1);
  display.setTextColor(BLACK);
  display.setCursor(x0-2,y0+r-8);
  display.println("S");
 
  display.setTextSize(1);
  display.setTextColor(BLACK);
  display.setCursor((x0-r)+5,y0-3);
  display.println("W");
 
  // Triangle for direction
 
  // display.drawTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color)
  display.drawTriangle(0, 46, 20, 46, 10, 18, BLACK);
  // fillTriangle ( int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color)
  display.fillTriangle (0, 46, 20, 46, 10, 18, BLACK);
  display.display();
  delay(100);
  display.clearDisplay();

}

void ResetSetCoils (void)
{

    // RESET / SET Coils
    Wire.beginTransmission(I2ADDR);
    Wire.write(0x00);
    Wire.write(RESET_COIL);
    Wire.endTransmission();
    delay(1);
    Wire.beginTransmission(I2ADDR);
    Wire.write(0x00);
    Wire.write(SET_COIL);
    Wire.endTransmission();
    delay(1);
}