looks like you are converting azimuth to degrees twice
once here
Code:
 'Calculate Azimuth

  GOSUB GetAzimuth

  'Calculated Heading in Degrees

   brads = azimuth     'get angle
  'degr = brads */ 361 'convert to degrees
   degr = brads*360/256      and this is still wrong needs to be  degr = brads */ 360 

  
   Lcdout $fe, $C0, "Degrees =",SDEC degr
and once here


Code:
x = readx >>4  
Y = readY >>4
Z = readZ >>4

  'DEBUG       "SCALED X,Y", CR
  'DEBUG       "X = ",11, SDEC x, CR
  'DEBUG       "Y = ",11, SDEC y, CR

   'azimuth = x ATN -y
  azimuth = ( x ATN -y) */ 360
surly once is enough

Code:
azimuth = x ATN -y