'{$STAMP BS2}
' {$PBASIC 2.5}

'***********************************************************
'**                                                       **
'**      Demonstration I2C routines for controlling       **
'**    the MD23 with a basic stamp, displaying results    **
'**              in a PC debug window                     **
'**                                                       **
'**          Written by Chris Clarke - Aug 2008           **
'**                                                       **
'***********************************************************

enc1a      VAR     Byte
enc1b      VAR     Byte
enc1c      VAR     Byte
enc1d      VAR     Byte
enc2a      VAR     Byte
enc2b      VAR     Byte
enc2c      VAR     Byte
enc2d      VAR     Byte
bat_volts  VAR     Byte
mot1_cur   VAR     Byte
mot2_cur   VAR     Byte
ver        VAR     Byte

SDA     CON 3      ' I2C data
SCL     CON 4      ' I2C clock
SDAin   VAR IN3
SDAout  VAR OUT3
SDAdir  VAR DIR3
I2cBuf  VAR Byte      ' I2c read/write buffer
I2cAddr VAR Byte      ' Address of I2C device
I2cReg  VAR Byte      ' Register number within I2C device
I2cData VAR Word      ' Data to read/write
I2cAck  VAR Bit       ' Acknowledge bit

Mode_reg      CON     14
Version_reg   CON     13
Speed1_reg    CON      0
Speed2_reg    CON      1
Enc1_reg      CON      2
Command_reg   CON     16
ZeroEncs      CON     32

Main:
  I2caddr = $b0
  ver = 255

  DO WHILE (ver = 255)
    DEBUG 2,1,1, "Scanning for md23"
    DEBUG 2,1,2, "address = 0x",HEX I2caddr
    I2cReg = Version_reg      'point the register read at the version
    GOSUB I2CByteRead
    ver = I2cData
    IF(ver = 255) THEN I2cAddr = I2cAddr + 2
    IF (I2cAddr = $c0) THEN I2cAddr = $b0
    PAUSE 100
  LOOP   ' Loop forever

MainLoop:
  I2cReg = Mode_reg    'set mode 0 (actually unnecessary, as its a power-up default
  I2cdata = 0
  GOSUB I2cByteWrite
  I2cReg = Command_reg
  I2cdata = ZeroEncs
  GOSUB I2cByteWrite
  I2cReg = Speed1_reg
  I2cdata = 192          'set motor 1 traveling forwards at half speed
  GOSUB I2cByteWrite
  I2cReg = Speed2_reg
  I2cdata = 64           'set motor 2 traveling backwards at half speed
  GOSUB I2cByteWrite
  DEBUG CLS
  DO WHILE 1
    GOSUB MD23_read_data
    IF(ver = 255) THEN GOTO Main    'it's 255 if there is no response from the MD23 module, so back to address search
    DEBUG 2,1,0, "MD23 address = 0x",HEX I2caddr
    DEBUG 2,1,1,"Motor1 Current ", DEC mot1_cur," "
    DEBUG 2,1,2,"Motor2 Current ", DEC mot2_cur," "
    DEBUG 2,1,3,"Battery Volts ", DEC bat_volts/10,".",DEC bat_volts//10,"V"
    IF (enc1a > 127) THEN DEBUG 2,25,0,"Encoder1 -", HEX 255-enc1a,HEX 255-enc1b,HEX 255-enc1c,HEX 255-enc1d
    IF (enc1a < 127) THEN DEBUG 2,25,1,"Encoder1  ", HEX enc1a,HEX enc1b,HEX enc1c,HEX enc1d
    IF (enc2a > 127) THEN DEBUG 2,25,2,"Encoder2 -", HEX 255-enc2a,HEX 255-enc2b,HEX 255-enc2c,HEX 255-enc2d," "
    IF (enc2a < 127) THEN DEBUG 2,25,3,"Encoder2  ", HEX enc2a,HEX enc2b,HEX enc2c,HEX enc2d," "

  LOOP   ' Loop forever

MD23_read_data:            ' grabs all the data I want, note that the I2CByteRead routine increments the register pointer after a read

    I2cReg = Enc1_reg      'point to encoder 1 register, high byte first
    GOSUB I2CByteRead
    enc1a = I2cData
    GOSUB I2CByteRead
    enc1b = I2cData
    GOSUB I2CByteRead
    enc1c = I2cData
    GOSUB I2CByteRead
    enc1d = I2cData
    GOSUB I2CByteRead
    enc2a = I2cData
    GOSUB I2CByteRead
    enc2b = I2cData
    GOSUB I2CByteRead
    enc2c = I2cData
    GOSUB I2CByteRead
    enc2d = I2cData
    GOSUB I2CByteRead
    bat_volts = I2cData
    GOSUB I2CByteRead
    mot1_cur = I2cData
    GOSUB I2CByteRead
    mot2_cur = I2cData
    GOSUB I2CByteRead
    ver = I2cData
  '--------------------------------------------------------------------------------------------
' I2C subroutines follow
'--------------------------------------------------------------------------------------------
I2cByteWrite:        ' writes I2cData to I2cReg at I2cAddr
  GOSUB  I2cStart
  I2cBuf = I2cAddr
  GOSUB  I2cOutByte      ' send device address
  I2cBuf = I2cReg
  GOSUB  I2cOutByte      ' send register number
  I2cBuf = I2cData
  GOSUB  I2cOutByte      ' send the data
  GOSUB  I2cStop
  RETURN

I2CByteRead:
  GOSUB  I2cStart
  I2cBuf = I2cAddr
  GOSUB  I2cOutByte      ' send device address
  I2cBuf = I2cReg
  GOSUB  I2cOutByte      ' send register number
  GOSUB  I2cStart      ' repeated start
  I2cBuf = I2cAddr | 1
  GOSUB  I2cOutByte      ' send device address (with read set)
  I2cAck = 0        ' send Nak
  GOSUB  I2cInByte
  I2cData = I2cBuf  ' read the data
  GOSUB  I2cStop
  I2cReg = I2creg + 1 'increment register pointer for next read, so sequential reads can be made if needed
  RETURN

I2cOutByte:
  SHIFTOUT SDA, SCL, MSBFIRST, [I2cBuf]
  INPUT  SDA
  HIGH  SCL          ' clock in the ack' bit
  LOW  SCL
  RETURN

I2cInByte:
  SHIFTIN SDA, SCL, MSBPRE, [I2cBuf]
  SDAout = 0
  SDAdir = I2cAck
  HIGH  SCL          ' clock out the ack' bit
  LOW   SCL
  INPUT  SDA
  RETURN

I2cStart:            ' I2C start bit sequence
  HIGH  SDA
  HIGH  SCL
  LOW  SDA
  LOW  SCL
  RETURN

I2cStop:            ' I2C stop bit sequence
  LOW  SDA
  HIGH  SCL
  HIGH  SDA
  RETURN