'***********************************************************
'**                                                       **
'**            I2C Routines for the BX-24                 **
'**      to demonstrate the use of Devantech SD21         **
'**           21 Channel Servo Driver Module              **
'**                                                       **
'**            Copyright 2004 - Luigi Carnevale           **
'**     Commercial use of this software is prohibited     **
'**     Private and educational use only is permitted     **
'**                                                       **
'**                                                       **
'***********************************************************

Option Explicit

Const SCL As Byte = 6                  ' I2C clock - SCL
Const SDA As Byte = 5                  ' I2C data - SDA

Const SD21 As Byte = &HC2 'SD21 I2CAddress

Const SV1Base As Byte = 63 ' Servo 1 Base Register
Const SV1PO As Byte = 84 ' Servo 1 Positive Offset
Const SV1NO As Byte = 105 ' Servo 1 Negative Offset
Const SV1Centre As Byte = 128 ' Servo 1 Centre Position

Sub Main()


    Call PutPin(SCL, bxOutputHigh)
    Call PutPin(SDA, bxOutputHigh)

    Call I2cByteWrite(SD21, SV1Base, SV1Centre)
    Delay(1.0)

Do
   Call I2cByteWrite(SD21, SV1PO, 50)
Delay(1.0)
Call I2cByteWrite(SD21, SV1NO, 50)
Delay(1.0)
Loop

End Sub


'--------------------------------
'I2C Sub Routines by Gerald Coe
'--------------------------------

' writes I2cData to I2cReg at I2cAddr

Sub I2cByteWrite(ByVal I2cAddr As Byte, ByVal I2cReg As Byte, ByVal I2cData As Byte)
    Call I2cStart()
    Call I2cOutByte(I2cAddr)                   ' send device address
    Call I2cOutByte(I2cReg)                    ' send register address
    Call I2cOutByte(I2cData)                   ' send the data
    Call I2cStop()
End Sub

Sub I2cOutByte(I2cData As Byte)
    Call ShiftOut(SDA, SCL, 8, I2cData)        ' shift data out
    Call PutPin(SDA, bxInputTristate)          ' turn SDA around
    Call PutPin(SCL, bxOutputHigh)             ' and clock in the ack' bit
    Call PutPin(SCL, bxOutputLow)
End Sub

Sub I2cStart()                                 ' I2C start bit sequence
    Call PutPin(SDA, bxOutputHigh)
    Call PutPin(SCL, bxOutputHigh)
    Call PutPin(SDA, bxOutputLow)
    Call PutPin(SCL, bxOutputLow)
End Sub

Sub I2cStop()                                  ' I2C stop bit sequence
    Call PutPin(SDA, bxOutputLow)
    Call PutPin(SCL, bxOutputHigh)
    Call PutPin(SDA, bxOutputHigh)
End Sub