diff --git a/README.md b/README.md
index 03a63b2..9ce55a6 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
-![image](http://www.4dsystems.com.au/downloads/4DLogo.png)
+![4D Logo](./logo.jpg)
 
 Diablo16-Serial-Arduino-Library
 =============================
@@ -7,15 +7,18 @@ Arduino Library for 4D Systems Serial Environment for Diablo16 based modules
 
 ## Information
 
-This library is provided with the installation of Workshop 4 from 4D Systems, however can be downloaded here for people who do not run Windows.
 This library provides high level functions for the Arduino, to ease communication with Diablo16 based modules when using the module configured for Serial.
-Please refer to the 4D Systems website, namingly the Workshop 4 Product Page, for documentation regarding Workshop 4, and its environments.
+Please refer to the 4D Systems website, namingly the Workshop4 Product Page, for documentation regarding Workshop4, and its environments.
 
 ## Installation
 
-Library should be placed in the C:\Users\(User name)\My Documents\Arduino\Libraries\ folder, or equivalent. (restart the IDE if already open)
+It is advisable to simply use the Arduino IDE's library manager to search and install Picaso Serial Arduino Library.
+
+For more information on the installation, please refer to the following documentation from Arduino:
+
+- Arduino 1.x: [Installing Libraries](https://docs.arduino.cc/software/ide-v1/tutorials/installing-libraries)
+- Arduino 2.x: [Installing Libraries](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library) 
 
-For more information on the installation, please refer to [Installing Additional Arduino Libraries] (http://arduino.cc/en/Guide/Libraries)
 
 ## Example Sketch
 
diff --git a/library.properties b/library.properties
index f169430..af74098 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=Diablo16-Serial-Arduino-Library
-version=1.0.4
+version=1.0.5
 author=4D Systems
 maintainer=4D Systems
 sentence=Provides library access to communicate with the 4D Systems Diablo16 processor, when configured in Serial/SPE mode
diff --git a/logo.jpg b/logo.jpg
new file mode 100644
index 0000000..3e65831
Binary files /dev/null and b/logo.jpg differ
diff --git a/src/Diablo_ColourRoutines.cpp b/src/Diablo_ColourRoutines.cpp
new file mode 100644
index 0000000..dca3249
--- /dev/null
+++ b/src/Diablo_ColourRoutines.cpp
@@ -0,0 +1,126 @@
+#if (ARDUINO >= 100)
+	#include "Arduino.h" // for Arduino 1.0
+#else
+	#include "WProgram.h" // for Arduino 23
+#endif
+
+//#include "Picaso_Serial_4DLib.h"
+//#include "Picaso_Const4D.h"
+
+#define HLSMAX     127                     // HLS vary over 0-HLSMAX
+#define HLSMAXm2d3 (HLSMAX* 2 / 3 )
+#define HLSMAXd12  (HLSMAX / 12 )
+#define HLSMAXd6   (HLSMAX / 6 )
+#define HLSMAXd2   (HLSMAX / 2 )
+#define HLSMAXd3   (HLSMAX / 3 )
+#define RGBMAX     127                     // R,G, and B vary over 0-RGBMAX
+#define RGBMAXm2   (RGBMAX*2) 
+// HLSMAX BEST IF DIVISIBLE BY 6
+// RGBMAX, HLSMAX must each fit in a byte.
+
+// Hue is undefined if Saturation is 0 (grey-scale) *
+// This value determines where the Hue scrollbar is
+// initially set for achromatic colors
+#define UNDEFINED  (127 * 2 / 3 ) 
+
+// color conversion routines seem to be based on http://support.microsoft.com/kb/29240
+// My copy seem to have been around since the MSDOS days, I don't know if it came from another source
+
+uint16_t RGBs2COL(int16_t r, int16_t g, int16_t b)
+{
+    return (b >> 2) | (g & 0x7E) << 4 | (r & 0x7c) << 9  ;
+}
+
+void c565toRGBs(int16_t i565, int16_t * red, int16_t * green, int16_t * blue)
+{
+    *red   = (i565 & 0xF800) >> 9 ;
+    *green = (i565 & 0x07E0) >> 4 ;
+    *blue  = (i565 & 0x001F) << 2 ;
+}
+
+void RGB2HLS(int16_t red, int16_t green, int16_t blue, int16_t * h, int16_t * l, int16_t * s)
+{
+    int16_t  cMax, cMin, Rdelta, Gdelta, Bdelta, cMpM, cMmM ;
+    // calculate lightness
+    cMax = max( max(red,green), blue);
+    cMin = min( min(red,green), blue);
+    cMpM = cMax+cMin ;
+    cMmM = cMax-cMin ;
+
+    *l = ( (cMpM*HLSMAX) + RGBMAX ) / RGBMAXm2;
+
+    if (cMax == cMin) // r=g=b --> achromatic case
+    {
+        *s = 0;
+        *h = UNDEFINED;
+    }
+    else // chromatic case
+    {
+        // saturation
+        if (*l <= (HLSMAX/2))
+            *s = ( (cMmM*HLSMAX) + (cMpM / 2) ) / cMpM ;
+        else
+            *s = ( (cMmM*HLSMAX) + ((RGBMAXm2-cMpM) / 2) ) / (RGBMAXm2-cMpM);
+
+        // hue
+        Rdelta = ( ((cMax-red)* HLSMAXd6) + (cMmM / 2) ) / cMmM;
+        Gdelta = ( ((cMax-green)* HLSMAXd6) + (cMmM / 2) ) / cMmM;
+        Bdelta = ( ((cMax-blue)* HLSMAXd6) + (cMmM / 2) ) / cMmM;
+
+        if (red == cMax)
+            *h = Bdelta - Gdelta ;
+        else if (green == cMax)
+            *h = HLSMAXd3 + Rdelta - Bdelta ;
+        else
+            *h = HLSMAXm2d3 + Gdelta - Rdelta;
+
+        if (*h < 0) *h += HLSMAX;
+        if (*h > HLSMAX) *h -= HLSMAX;
+    }
+}
+
+int16_t hue_RGB(int16_t Hin, int16_t M1, int16_t M2)
+{
+    int16_t Value ;
+    if (Hin < 0)
+        Hin += HLSMAX ;
+    else if (Hin > HLSMAX)
+        Hin -= HLSMAX ;
+
+    if (Hin < HLSMAXd6)
+        Value = M1 + ( (M2 - M1) * Hin + HLSMAXd12 ) / HLSMAXd6 ;
+    else if (Hin < HLSMAXd2 )
+        Value = M2 ;
+    else if (Hin < HLSMAXm2d3)
+        Value = M1 + ( (M2 - M1) * (HLSMAXm2d3 - Hin) + HLSMAXd12) / HLSMAXd6 ;
+    else
+        Value = M1 ;
+    return Value ;
+}
+
+void HLS2RGB(int16_t H, int16_t L, int16_t S, int16_t * red, int16_t * green, int16_t * blue)
+{
+    int16_t M1, M2 ;
+
+    if (S == 0)
+    {
+        *red   = L ;
+        *green = L ;
+        *blue  = L ;
+    }
+    else
+    {
+        if (L <= HLSMAXd2)
+            M2 = (L * (HLSMAX + S) + HLSMAXd2) / HLSMAX ;
+        else
+            M2 = L + S - ((L * S + HLSMAXd2) / HLSMAX) ;
+
+        M1 = 2 * L - M2 ;
+        // Determine levels of primary colours.
+        if ((H > HLSMAX ) || (H < 0)) H = 0 ;
+        *red   = hue_RGB( H+HLSMAXd3, M1, M2 ) ;
+        *green = hue_RGB( H,          M1, M2 ) ;
+        *blue  = hue_RGB( H-HLSMAXd3, M1, M2 ) ;
+    }
+
+ }
diff --git a/src/Diablo_ColourRoutines.h b/src/Diablo_ColourRoutines.h
new file mode 100644
index 0000000..6fe3aa7
--- /dev/null
+++ b/src/Diablo_ColourRoutines.h
@@ -0,0 +1,28 @@
+// color conversion routines seem to be based on http://support.microsoft.com/kb/29240
+// My copy seem to have been around since the MSDOS days, I don't know if it came from another source
+#define HLSMAX     127                     // HLS vary over 0-HLSMAX
+#define HLSMAXm2d3 HLSMAX* 2 / 3 
+#define HLSMAXd12  HLSMAX / 12 
+#define HLSMAXd6   HLSMAX / 6 
+#define HLSMAXd2   HLSMAX / 2 
+#define HLSMAXd3   HLSMAX / 3 
+#define RGBMAX     127                     // R,G, and B vary over 0-RGBMAX
+#define RGBMAXm2   RGBMAX*2 
+// HLSMAX BEST IF DIVISIBLE BY 6
+// RGBMAX, HLSMAX must each fit in a byte.
+
+// Hue is undefined if Saturation is 0 (grey-scale) *
+// This value determines where the Hue scrollbar is
+// initially set for achromatic colors
+#define UNDEFINED  127 * 2 / 3  
+
+uint16_t RGBs2COL(int16_t r, int16_t g, int16_t b) ;
+
+void c565toRGBs(int16_t i565, int16_t * red, int16_t * green, int16_t * blue);
+
+void RGB2HLS(int16_t red, int16_t green, int16_t blue, int16_t * h, int16_t * l, int16_t * s) ;
+
+uint16_t hue_RGB(int16_t Hin, int16_t M1, int16_t M2) ;
+
+void HLS2RGB(int16_t H, int16_t L, int16_t S, int16_t * r, int16_t * g, int16_t * b);
+
diff --git a/src/Diablo_KBRoutines.cpp b/src/Diablo_KBRoutines.cpp
new file mode 100644
index 0000000..53265ac
--- /dev/null
+++ b/src/Diablo_KBRoutines.cpp
@@ -0,0 +1,131 @@
+#include "Diablo_Serial_4DLib.h"
+#include "Diablo_Const4D.h"
+
+#include "Diablo_KBRoutines.h"
+
+#define KbShiftBit        01
+#define KbCapsBit         02
+#define KbShiftCapsBits   03
+#define KbCtrlBit         04
+
+#if (ARDUINO >= 100)
+ #include "Arduino.h" // for Arduino 1.0
+#else
+ #include "WProgram.h" // for Arduino 23
+#endif
+
+void refreshstate(Diablo_Serial_4DLib Display, word hndl, int8_t iKB, int8_t * oKB)
+{
+  int8_t shifted ;
+  shifted = oKB[KbState] & KbShiftCapsBits ;
+  if (!shifted || (shifted == KbShiftCapsBits))
+  {
+    shifted = 0 ;
+    oKB[KbCaps] = 0 ;
+  }
+  else
+  {
+    shifted = 1 ;
+    oKB[KbCaps] = 2 ;
+  }
+  setkeystate(Display, hndl, iKB, shifted) ;
+  if (oKB[KbState] & KbCapsBit)
+    setkeystate(Display, hndl, iKB + oKB[KbLock],1) ;
+  if ((oKB[KbState] & KbShiftBit) && (shifted))
+  {
+    setkeystate(Display, hndl, iKB + oKB[KbShift1],1) ;
+    setkeystate(Display, hndl, iKB + oKB[KbShift2],1) ;
+  }
+  if (oKB[KbState] & KbCtrlBit)
+  {
+    setkeystate(Display, hndl, iKB +oKB[KbCtrl1],1) ;
+    setkeystate(Display, hndl, iKB + oKB[KbCtrl2],1) ;
+  }
+}
+
+
+void kbDown(Diablo_Serial_4DLib Display, word hndl, int8_t iKB, int8_t * oKB, uint8_t * KBKeys, int8_t key, Callback handler)
+{
+  int8_t keyval ;
+  oKB[KbMvt] = 1 ;
+  oKB[KbIgn] = 0 ;
+  if ((key == oKB[KbShift1]) || (key == oKB[KbShift2]))
+  {
+    if (oKB[KbState] & KbShiftBit)
+    {
+      oKB[KbState] &= ~KbShiftBit ;
+      oKB[KbMvt] = 0 ;
+    }
+    else
+      oKB[KbState] |= KbShiftBit ;
+    refreshstate(Display, hndl, iKB, oKB) ;
+    oKB[KbIgn] = 1 ;
+  }
+  else if ((key == oKB[KbCtrl1]) || (key == oKB[KbCtrl2]))
+  {
+    if (oKB[KbState] & KbCtrlBit)
+    {
+      oKB[KbState] &= ~KbCtrlBit ;
+      oKB[KbMvt] = 0 ;
+    }
+    else
+      oKB[KbState] |= KbCtrlBit ;
+    setkeystate(Display, hndl, iKB + oKB[KbCtrl1],oKB[KbMvt]) ;
+    key = oKB[KbCtrl2] ;
+    oKB[KbIgn] = 1 ;
+  }
+  else if (key == oKB[KbLock])
+  {
+    if (oKB[KbState] & KbCapsBit)
+    {
+      oKB[KbState] &= ~KbCapsBit ;
+      oKB[KbMvt] = 0 ;
+    }
+    else
+      oKB[KbState] |= KbCapsBit ;
+    refreshstate(Display, hndl, iKB, oKB) ;
+    oKB[KbIgn] = 1 ;
+  }
+
+  if (!oKB[KbIgn])
+  {
+    if (oKB[KbShiftCaps])
+      keyval = (oKB[KbState] & KbShiftCapsBits) * oKB[KbButtons] - 1 ;
+    else if (((oKB[KbState] & KbShiftCapsBits) == 0) || ((oKB[KbState] & KbShiftCapsBits) == KbShiftCapsBits))
+      keyval = - 1 ;
+    else
+      keyval = oKB[KbButtons] - 1 ;
+    keyval = KBKeys[key+keyval] ;
+    if (oKB[KbState] & KbCtrlBit) keyval &= 0x9F ;
+    handler(keyval & 0xFF) ;
+    setkeystate(Display, hndl, iKB + key,oKB[KbMvt]+oKB[KbCaps]) ;
+  }
+  oKB[KbDown] = key ;
+}
+
+void setkeystate(Diablo_Serial_4DLib Display, word hndl, int8_t key, int8_t idx)
+{
+  Display.img_SetWord(hndl, key,IMAGE_INDEX, idx);
+  Display.img_Show(hndl,key) ;
+}
+
+void kbUp(Diablo_Serial_4DLib Display, word hndl, int8_t iKB, int8_t * oKB)
+{
+  if (!oKB[KbIgn])
+  {
+    setkeystate(Display, hndl, iKB + oKB[KbDown], oKB[KbCaps]) ;
+    if (oKB[KbState] & KbShiftBit)
+    {
+      oKB[KbState] &= ~KbShiftBit ;
+      refreshstate(Display, hndl, iKB, oKB) ;
+    }
+    if (oKB[KbState] & KbCtrlBit)
+    {
+      oKB[KbState] &= ~KbCtrlBit ;
+      setkeystate(Display, hndl, iKB + oKB[KbCtrl1],0) ;
+      setkeystate(Display, hndl, iKB + oKB[KbCtrl2],0) ;
+    }
+    oKB[KbDown] = -1 ;
+  }
+}
+
diff --git a/src/Diablo_KBRoutines.h b/src/Diablo_KBRoutines.h
new file mode 100644
index 0000000..1d2aa75
--- /dev/null
+++ b/src/Diablo_KBRoutines.h
@@ -0,0 +1,30 @@
+#if defined(ARDUINO) && ARDUINO >= 100
+  #include "Arduino.h"
+#else
+  #include "WProgram.h"
+#endif
+
+#define KbDown      0
+#define KbMvt       1
+#define KbState     2
+#define KbIgn       3
+#define KbCaps      4
+#define KbShift1    5
+#define KbShift2    6
+#define KbCtrl1     7
+#define KbCtrl2     8
+#define KbLock      9
+#define KbButtons   10
+#define KbShiftCaps 11
+
+typedef void        (*Callback) (int);
+
+void refreshstate(Diablo_Serial_4DLib Display, word hndl, int8_t iKB, int8_t * oKB) ;
+
+void kbDown(Diablo_Serial_4DLib Display, word hndl, int8_t iKB, int8_t * oKB, uint8_t * KBKeys, int8_t key, Callback handler) ;
+
+void kbUp(Diablo_Serial_4DLib Display, word hndl, int8_t iKB, int8_t * oKB) ;
+
+void setkeystate(Diablo_Serial_4DLib Display, word hndl, int8_t key, int8_t idx) ;
+
+
diff --git a/src/Diablo_LedDigitsDisplay.cpp b/src/Diablo_LedDigitsDisplay.cpp
new file mode 100644
index 0000000..dd9385d
--- /dev/null
+++ b/src/Diablo_LedDigitsDisplay.cpp
@@ -0,0 +1,35 @@
+#if (ARDUINO >= 100)
+	#include "Arduino.h" // for Arduino 1.0
+#else
+	#include "WProgram.h" // for Arduino 23
+#endif
+
+#include "Diablo_Serial_4DLib.h"
+#include "Diablo_Const4D.h"
+
+// WARNING, this code will crash if newval exceeds maximum displayable number
+void LedDigitsDisplay(Diablo_Serial_4DLib Display, word hndl, word newval, word index, word left, word Digits, word MinDigits, word WidthDigit, word LeadingBlanks)
+{
+  word i, k, l, lb ;
+  l = 1 ;
+  for (i = 1; i < Digits; i++)
+    l *= 10 ;
+  lb = LeadingBlanks ;
+  for (i = 0; i < Digits; i++)
+  {
+    k = newval / l ;
+    newval -= k * l ;
+    if ( lb && (i < Digits - MinDigits) )
+    { 
+      if (k == 0)
+        k = 10 ;
+      else
+        lb = 0 ;
+    }
+    l /= 10 ;
+    Display.img_SetWord(hndl, index, IMAGE_INDEX, k);
+    Display.img_SetWord(hndl, index, IMAGE_XPOS, left+i*WidthDigit) ;
+    Display.img_Show(hndl, index);
+  }
+}
+
diff --git a/src/Diablo_LedDigitsDisplay.h b/src/Diablo_LedDigitsDisplay.h
new file mode 100644
index 0000000..0ad3214
--- /dev/null
+++ b/src/Diablo_LedDigitsDisplay.h
@@ -0,0 +1,8 @@
+#if (ARDUINO >= 100)
+	#include "Arduino.h" // for Arduino 1.0
+#else
+	#include "WProgram.h" // for Arduino 23
+#endif
+
+// WARNING, this code will crash if newval exceeds maximum displayable number
+void LedDigitsDisplay(Diablo_Serial_4DLib Display, word hndl, word newval, word index, word left, word Digits, word MinDigits, word WidthDigit, word LeadingBlanks) ;
diff --git a/src/Diablo_LedDigitsDisplaySigned.cpp b/src/Diablo_LedDigitsDisplaySigned.cpp
new file mode 100644
index 0000000..3dcc6cc
--- /dev/null
+++ b/src/Diablo_LedDigitsDisplaySigned.cpp
@@ -0,0 +1,48 @@
+#if (ARDUINO >= 100)
+	#include "Arduino.h" // for Arduino 1.0
+#else
+	#include "WProgram.h" // for Arduino 23
+#endif
+
+#include "Diablo_Serial_4DLib.h"
+#include "Diablo_Const4D.h"
+
+// Warning, ensure correct number of digits are specified, especially if -ve numbers are required as minus sign
+// will overlay most significant digit if there are not enough digits.
+void LedDigitsDisplaySigned(Diablo_Serial_4DLib Display, word hndl, int16_t newval, word index, word left, word Digits, word MinDigits, word WidthDigit, word LeadingBlanks)
+{
+  int16_t i, m, lstb, nv;
+
+  left = left + WidthDigit * (Digits-1) ;
+  nv = newval ;
+  lstb = 1 ;
+  for (i = Digits; i > 0; i--)
+  {
+    m = nv % 10 ;
+    if ( LeadingBlanks && (i <= Digits - MinDigits) )
+    {
+      if (nv == 0)
+      {
+        m = 10 ;
+        if (lstb == 1) 
+          lstb = i ;
+      }
+    }
+ 
+
+    Display.img_SetWord(hndl, index, IMAGE_INDEX, abs(m));
+    Display.img_SetWord(hndl, index, IMAGE_XPOS, left) ;
+    Display.img_Show(hndl, index);
+    nv /= 10 ;
+  
+    left -= WidthDigit ;
+  }
+  if (newval < 0)
+  {
+    left += lstb * WidthDigit ;
+    Display.img_SetWord(hndl, index, IMAGE_INDEX, 11);
+    Display.img_SetWord(hndl, index, IMAGE_XPOS, left) ;
+    Display.img_Show(hndl, index);
+  }
+
+}
\ No newline at end of file
diff --git a/src/Diablo_LedDigitsDisplaySigned.h b/src/Diablo_LedDigitsDisplaySigned.h
new file mode 100644
index 0000000..1f98f74
--- /dev/null
+++ b/src/Diablo_LedDigitsDisplaySigned.h
@@ -0,0 +1,9 @@
+#if (ARDUINO >= 100)
+	#include "Arduino.h" // for Arduino 1.0
+#else
+	#include "WProgram.h" // for Arduino 23
+#endif
+
+// Warning, ensure correct number of digits are specified, especially if -ve numbers are required as minus sign
+// will overlay most significant digit if there are not enough digits.
+void LedDigitsDisplaySigned(Diablo_Serial_4DLib Display, word hndl, int16_t newval, word index, word left, word Digits, word MinDigits, word WidthDigit, word LeadingBlanks) ;
diff --git a/src/Diablo_PrintDisk.cpp b/src/Diablo_PrintDisk.cpp
new file mode 100644
index 0000000..a4f93a7
--- /dev/null
+++ b/src/Diablo_PrintDisk.cpp
@@ -0,0 +1,39 @@
+#if (ARDUINO >= 100)
+  #include "Arduino.h" // for Arduino 1.0
+#else
+  #include "WProgram.h" // for Arduino 23
+#endif
+
+#include "Diablo_Serial_4DLib.h"
+#include "Diablo_Const4D.h"
+
+void PrintDisk(Diablo_Serial_4DLib Display, word hndl, long offset, word msglen, word msgid)
+{
+  char ch ;
+  long res ;
+  res = (long)msglen * msgid ;
+  res += offset ;
+  Display.file_Seek(hndl, res >> 16, res & 0xffff);
+  do
+  {
+    ch = Display.file_GetC(hndl) ;
+    Display.putCH(ch) ;
+  }
+  while (ch != 0) ;
+}
+
+void PrintDiskUnicode(Diablo_Serial_4DLib Display, word hndl, long offset, word msglen, word msgid)
+{
+  word ch ;
+  long res ;
+  res = (long)msglen * msgid ;
+  res += offset ;
+  Display.file_Seek(hndl, res >> 16, res & 0xffff);
+  do
+  {
+    ch = Display.file_GetW(hndl) ;
+    Display.putCH(ch) ;
+  }
+  while (ch != 0) ;
+}
+
diff --git a/src/Diablo_PrintDisk.h b/src/Diablo_PrintDisk.h
new file mode 100644
index 0000000..a694ba6
--- /dev/null
+++ b/src/Diablo_PrintDisk.h
@@ -0,0 +1,5 @@
+
+void PrintDisk(Diablo_Serial_4DLib Display, word hndl, long offset, word msglen, word msgid);
+
+void PrintDiskUnicode(Diablo_Serial_4DLib Display, word hndl, long offset, word msglen, word msgid);
+
diff --git a/src/Diablo_XYposToDegree.cpp b/src/Diablo_XYposToDegree.cpp
new file mode 100644
index 0000000..b2f4190
--- /dev/null
+++ b/src/Diablo_XYposToDegree.cpp
@@ -0,0 +1,71 @@
+#if defined(ARDUINO) && ARDUINO >= 100
+  #include "Arduino.h"
+#else
+  #include "WProgram.h"
+#endif
+//
+// Routine to convert X/Y (Relative to 0,0) position into Degrees
+// 0 Degrees is straight down to suit rotary objects
+//
+
+const uint8_t arctan[] = {0,  1,  1,  2,  2,  3,  3,  4,  5,  5,
+                 6,  6,  7,  7,  8,  9,  9, 10, 10, 11,
+                11, 12, 12, 13, 13, 14, 15, 15, 16, 16,
+                17, 17, 18, 18, 19, 19, 20, 20, 21, 21,
+                22, 22, 23, 23, 24, 24, 25, 25, 26, 26,
+                27, 27, 27, 28, 28, 29, 29, 30, 30, 31,
+                31, 31, 32, 32, 33, 33, 33, 34, 34, 35,
+                35, 35, 36, 36, 37, 37, 37, 38, 38, 38,
+                39, 39, 39, 40, 40, 40, 41, 41, 41, 42,
+                42, 42, 43, 43, 43, 44, 44, 44, 44, 45, 45} ;
+
+int16_t XYposToDegree(int16_t curX, int16_t curY)
+{
+  int16_t delta, deg, adj ;
+  if (curY < 0)
+  {
+    if (curX < 0)
+    {
+      adj = 1 ;
+      deg = 90 ;
+    }
+    else
+    {
+      adj = 2 ;
+      deg = 180 ;
+    }
+  }
+  else
+  {
+    if (curX < 0)
+    {
+      deg = 0 ;
+      adj = 2 ;
+    }
+    else
+    {
+      deg = 270 ;
+      adj = 1 ;
+    }
+  }
+
+  curX = abs(curX) ;
+  curY = abs(curY) ;
+  if (curX < curY)
+    adj &= 1 ;
+  else
+  {
+    adj &= 2 ;
+    delta = curX ;
+    curX = curY ;
+    curY = delta ;
+  }
+  delta = arctan[(curX * 100) / curY] ;
+  if (adj)
+    deg += 90 - delta ;
+  else
+    deg += delta ;
+
+  return deg ;
+}
+
diff --git a/src/Diablo_XYposToDegree.h b/src/Diablo_XYposToDegree.h
new file mode 100644
index 0000000..86753d8
--- /dev/null
+++ b/src/Diablo_XYposToDegree.h
@@ -0,0 +1 @@
+int16_t XYposToDegree(int16_t curX, int16_t curY) ;