'===============================================================================
' Project:  L3G4200D 3tripple axes gyro Test #1
' Created:  2011-12-23
' Edited:   2011-12-30
' By:       Glen Aidukas [bahbots.com]
' Description:
'  This is test software for initializing, reading and displaying data from
'  the L3G4200D triple axes gyro via I2C on an ATMega1284P
'  Note: ANSI (vt100) escape sequences are used to format output.
'  Make sure to use ANSI or vt100 terminal type for proper formating.
'  Breakout board is from: http://www.dsscircuits.com/l3g4200d-3-axis-gyro.html
'  Datasheet: http://www.dsscircuits.com/images/datasheets/L3G4200D.pdf
'-------------------------------------------------------------------------------

'=====[ Compiler Directives ]===================================================
$crystal    = 18432000
$regfile    = "m1284pdef.dat"
$baud       = 38400
$hwstack    = 64
$swstack    = 64
$framesize  = 64
'-------------------------------------------------------------------------------

'=====[ Global Vars & Constants ]===============================================
dim tick       as dword                   ' used for 100Hz tick count
dim tickn      as dword                   ' used for next timer tick event
dim n          as integer                 ' temp var for visual positioning

dim X          as integer
dim Xl         as byte at X + 0 overlay
dim Xh         as byte at X + 1 overlay

dim Y          as integer
dim Yl         as byte at Y + 0 overlay
dim Yh         as byte at Y + 1 overlay

dim Z          as integer
dim Zl         as byte at Z + 0 overlay
dim Zh         as byte at Z + 1 overlay

dim Temp       as byte
dim Status     as byte
dim Test_gyro  as Byte

'' list of L3G4200D register positions
Const WHO_AM_I       = &H0F

Const L3G4200D_R     = &HD3
Const L3G4200D_W     = &HD2

Const CTRL_REG1      = &H20
Const CTRL_REG2      = &H21
Const CTRL_REG3      = &H22
Const CTRL_REG4      = &H23
Const CTRL_REG5      = &H24
'-------------------------------------------------------------------------------

'=====[ Setup Timer0 for Tick isr ]=============================================
Config Timer2= Timer, Prescale= 1024, Compare A= Disconnect, Clear Timer= 1
' crystal  / Prescale / CountWanted = reload value
' 18432000 / 1024     / 100Hz       = 180 (trigger every 100th sec (10ms))
OCR2A = 180 - 1
On OC2A TickCounter_isr
Enable OC2A
'-------------------------------------------------------------------------------

'=====[ I2C BUS INIT ]==========================================================
'configure the scl and sda pins
$LIB "I2C_TWI.LBX"         ' uncomment for hardware I2C
'Config Scl = Portc.0       ' uncomment for software I2C
'onfig Sda = Portc.1        ' uncomment for software I2C
Config Twi = 400000
I2cinit
'-------------------------------------------------------------------------------

'=====[ Misc Initilization ]====================================================
Config Serialout  = Buffered, Size = 254
print "{027}[2J{027}[1;1H"; "Starting..."
Enable Interrupts
print
print "InitGyro: ";
gosub InitGyro
'-------------------------------------------------------------------------------

'=====[ Start of main loop ]====================================================
do
   if tick >= tickn then
      Gosub Read_Gyro

      print "{027}[5;1H"; "Count : "; tick;              "{027}[K"
      print                                              "{027}[K"
      print "Status: "; bin(status);                     "{027}[K"
      print "Temp  : "; temp;                            "{027}[K"
      print                                              "{027}[K"
      print "{027}[10;1H";
      print "Gyro-X: "; x;                               "{027}[K"
      print "Gyro-Y: "; y;                               "{027}[K"
      print "Gyro-Z: "; Z;                               "{027}[K"

      '' print visual position of values
      print "{027}[10;20H[";  : print "{027}[10;60H]";
      print "{027}[11;20H[";  : print "{027}[11;60H]";
      print "{027}[12;20H[";  : print "{027}[12;60H]";

      n= x/256 : n=n+ 40      : print "{027}[10;"; n; "H|";
      n= y/256 : n=n+ 40      : print "{027}[11;"; n; "H|";
      n= z/256 : n=n+ 40      : print "{027}[12;"; n; "H|";

      print "{027}[13;1H";                               "{027}[J";

      tickn= tickn + 20
   endif

   CONFIG POWERMODE = idle
Loop

End
'^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

'=====[ Tick timer ISR ]========================================================
TickCounter_isr:
   incr tick
return
'-------------------------------------------------------------------------------

'=====[ Initilize Gyro]=========================================================
InitGyro:
   I2cstart
    I2cwbyte L3G4200D_W
    I2cwbyte WHO_AM_I
   I2cstart
    I2cwbyte L3G4200D_R
    I2crbyte Test_gyro , Nack
   I2cstop

   Print "&B"; bin(Test_gyro); " [&h"; Hex(Test_Gyro); "]";
      ' if test_gyro = 11010011 (D3) then connection is ok

   I2cstart
    I2cwbyte L3G4200D_W
    I2cwbyte CTRL_REG1
    I2cwbyte &B00000_1111         'Enable x, y, z and turn off power down
   I2cstop

   I2cstart
    I2cwbyte L3G4200D_W
    I2cwbyte CTRL_REG2
    I2cwbyte &B0010_1001          'adjust/use the HPF
   I2cstop

   I2cstart
    I2cwbyte L3G4200D_W
    I2cwbyte CTRL_REG3
    I2cwbyte &B0000_1000
   I2cstop

   I2cstart
    I2cwbyte L3G4200D_W
    I2cwbyte CTRL_REG4
'    I2cwbyte &B0000_0000         'scale 250
'    I2cwbyte &B0001_0000         'scale 500
    I2cwbyte &B1011_0000         'scale 2000
   I2cstop

   I2cstart
    I2cwbyte L3G4200D_W
    I2cwbyte CTRL_REG5
    I2cwbyte &B0000_0000         'high-pass filtering of outputs
   I2cstop
return
'-------------------------------------------------------------------------------

'=====[ Read Gyro Data ]========================================================
Read_Gyro:
   I2cstart
    I2cwbyte L3G4200D_W
    I2cwbyte &B1010_0110         ' (OUT_TEMP AND &B10000000)
   I2cstart
    I2cwbyte L3G4200D_R
    I2crbyte Temp,    Ack
    I2crbyte Status,  Ack
    I2crbyte Xl,      Ack
    I2crbyte Xh,      Ack
    I2crbyte Yl,      Ack
    I2crbyte Yh,      Ack
    I2crbyte Zl,      Ack
    I2crbyte Zh,      Nack
   I2cstop
Return
'-------------------------------------------------------------------------------

n= x/256 : n=n+ 40      : print "{027}[10;"; n; "H|";
      n= y/256 : n=n+ 40      : print "{027}[11;"; n; "H|";
      n= z/256 : n=n+ 40      : print "{027}[12;"; n; "H|";

'===============================================================================
' Project:  L3G4200D 3tripple axes gyro Test #1
' Created:  2011-12-23
' Edited:   2011-12-30
' By:       Glen Aidukas [bahbots.com]
' Description:
'  This is test software for initializing, reading and displaying data from
'  the L3G4200D triple axes gyro via I2C on an ATMega1284P
'  Note: ANSI (vt100) escape sequences are used to format output.
'  Make sure to use ANSI or vt100 terminal type for proper formating.
'  Breakout board is from: http://www.dsscircuits.com/l3g4200d-3-axis-gyro.html
'  Datasheet: http://www.dsscircuits.com/images/datasheets/L3G4200D.pdf
'-------------------------------------------------------------------------------

'=====[ Compiler Directives ]===================================================
$crystal = 12000000
$regfile = "m32def.dat"
$baud = 9600
$hwstack = 64
$swstack = 64
$framesize = 64
'-------------------------------------------------------------------------------

'=====[ Global Vars & Constants ]===============================================
Dim Tick As Dword                                           ' used for 100Hz tick count
Dim Tickn As Dword                                          ' used for next timer tick event
Dim N1 As Integer , N2 As Integer , N3 As Integer           ' temp var for visual positioning

Dim Xg As Integer
Dim Xgl As Byte At Xg + 0 Overlay
Dim Xgh As Byte At Xg + 1 Overlay

Dim Yg As Integer
Dim Ygl As Byte At Yg + 0 Overlay
Dim Ygh As Byte At Yg + 1 Overlay

Dim Zg As Integer
Dim Zgl As Byte At Zg + 0 Overlay
Dim Zgh As Byte At Zg + 1 Overlay

Dim Temp As Byte
Dim Status As Byte
Dim Test_gyro As Byte

'' list of L3G4200D register positions
Const Who_am_i = &H0F

Const L3g4200d_r = &HD3
Const L3g4200d_w = &HD2

Const Ctrl_reg1 = &H20
Const Ctrl_reg2 = &H21
Const Ctrl_reg3 = &H22
Const Ctrl_reg4 = &H23
Const Ctrl_reg5 = &H24
'-------------------------------------------------------------------------------

'=====[ Setup Timer0 for Tick isr ]=============================================
Config Timer2 = Timer , Prescale = 1024                     ', Compare A= Disconnect, Clear Timer= 1
' crystal  / Prescale / CountWanted = reload value
' 18432000 / 1024     / 100Hz       = 180 (trigger every 100th sec (10ms))
Ocr2 = 117 - 1
On Oc1b Tickcounter_isr
Enable Oc1b
'-------------------------------------------------------------------------------

'=====[ I2C BUS INIT ]==========================================================
'configure the scl and sda pins
$lib "I2C_TWI.LBX"                                          ' uncomment for hardware I2C
Config Scl = Portc.0                                        ' uncomment for software I2C
Config Sda = Portc.1                                        ' uncomment for software I2C
Config Twi = 400000
I2cinit
'-------------------------------------------------------------------------------

'=====[ Misc Initilization ]====================================================
Config Serialout = Buffered , Size = 254
'print "{027}[2J{027}[1;1H"; "Starting..."
Enable Interrupts
Print "InitGyro: ";
Waitms 500
Gosub Initgyro
'-------------------------------------------------------------------------------

'=====[ Start of main loop ]====================================================
Cls
Do
   If Tick >= Tickn Then
      Gosub Read_gyro

      Print "{027}[5;1H" ; "Count : " ; Tick ; "{027}[K"
      Print "{027}[K"
      Print "Status: " ; Bin(status) ; "{027}[K"
      Print "Temp  : " ; Temp ; "{027}[K"
      Print "{027}[K"
      Print "{027}[10;1H";
      Print "Gyro-X: " ; Xg ; "  "                          '; "{027}[K"
      Print "Gyro-Y: " ; Yg ; "  "                          '; "{027}[K"
      Print "Gyro-Z: " ; Zg ; "  "                          '; "{027}[K"

      '' print visual position of values
      Print "{027}[10;20H[" ; : Print "{027}[10;60H]";
      Print "{027}[11;20H[" ; : Print "{027}[11;60H]";
      Print "{027}[12;20H[" ; : Print "{027}[12;60H]";

      Locate 1 , 1
      Lcd Xg ; "  " ; Yg ; "  " ; Zg ; "  "
      Locate 2 , 1
      Lcd Status ; "  " ; Test_gyro ; "  "

      N1 = Xg / 256 : N1 = N1 + 40                          ' : Print "{027}[10;" ; N1 ; "H|";
      N2 = Yg / 256 : N2 = N2 + 40                          ': Print "{027}[11;" ; N2 ; "H|";
      N3 = Zg / 256 : N3 = N3 + 40                          ': Print "{027}[12;" ; N3 ; "H|";
      Print "*" ; N1 ; "," ; N2 ; "," ; N3 ; "^"

      Print "{027}[13;1H" ; "{027}[J";

      Tickn = Tickn + 20
      'Waitms 50
   End If

   Config Powermode = Idle
Loop

End
'^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

'=====[ Tick timer ISR ]========================================================
Tickcounter_isr:
   Incr Tick
Return
'-------------------------------------------------------------------------------

'=====[ Initilize Gyro]=========================================================
Initgyro:
   I2cstart
    I2cwbyte L3g4200d_w
    I2cwbyte Who_am_i
   I2cstart
    I2cwbyte L3g4200d_r
    I2crbyte Test_gyro , Nack
   I2cstop

'  Print "&B"; bin(Test_gyro); " [&h"; Hex(Test_Gyro); "]";
      ' if test_gyro = 11010011 (D3) then connection is ok

   I2cstart
    I2cwbyte L3g4200d_w
    I2cwbyte Ctrl_reg1
    I2cwbyte &B00000_1111                                   'Enable x, y, z and turn off power down
   I2cstop

   I2cstart
    I2cwbyte L3g4200d_w
    I2cwbyte Ctrl_reg2
    I2cwbyte &B0010_1001                                    'adjust/use the HPF
   I2cstop

   I2cstart
    I2cwbyte L3g4200d_w
    I2cwbyte Ctrl_reg3
    I2cwbyte &B0000_1000
   I2cstop

   I2cstart
    I2cwbyte L3g4200d_w
    I2cwbyte Ctrl_reg4
    I2cwbyte &B0000_0000                                    'scale 250
'    I2cwbyte &B0001_0000         'scale 500
'    I2cwbyte &B1011_0000         'scale 2000
   I2cstop

   I2cstart
    I2cwbyte L3g4200d_w
    I2cwbyte Ctrl_reg5
    I2cwbyte &B0000_0000                                    'high-pass filtering of outputs
   I2cstop
Return
'-------------------------------------------------------------------------------

'=====[ Read Gyro Data ]========================================================
Read_gyro:
   I2cstart
    I2cwbyte L3g4200d_w
    I2cwbyte &B1010_0110                                    ' (OUT_TEMP AND &B10000000)
   I2cstart
    I2cwbyte L3g4200d_r
    I2crbyte Temp , Ack
    I2crbyte Status , Ack
    I2crbyte Xgl , Ack
    I2crbyte Xgh , Ack
    I2crbyte Ygl , Ack
    I2crbyte Ygh , Ack
    I2crbyte Zgl , Ack
    I2crbyte Zgh , Nack
   I2cstop
Return
'-------------------------------------------------------------------------------

Read_Gyro:
   I2cstart
    I2cwbyte L3G4200D_W
    I2cwbyte &B1010_0110         ' (OUT_TEMP AND &B10000000)   <--------------------------------
   I2cstart
    I2cwbyte L3G4200D_R
    I2crbyte Temp,    Ack
    I2crbyte Status,  Ack
    I2crbyte Xl,      Ack
    I2crbyte Xh,      Ack
    I2crbyte Yl,      Ack
    I2crbyte Yh,      Ack
    I2crbyte Zl,      Ack
    I2crbyte Zh,      Nack
   I2cstop
Return