/*
Program ID  : PortSplitter-03.nxc
Create date : 11-2-2010
Author      : CH, Chen (Taiwan)
Description :
Digital sensor mux testing(Mindsensors.com Port Splitter)

I2C Register Layout :
Register  Read                         Write
--------- ---------------------------  ---------------------
0x00-0x07 S/W version
0x08-0x0f Vendor ID
0x10-0x17 Device ID

*/
#define DEVICE_PORT     S1    //Connect sensor to this port
#define NXTHID_ID     0x04
#define US_ID         0x02
#define PFMATE_ID     0x48

#define SW_VERSION    0x00
#define VENDOR_ID     0x08
#define DEVICE_NAME   0x10

/* ********************************************************
 * Get sensor value
 * ******************************************************** */
 string GetSensor (const byte device_id)
 {
    byte I2C_req_buf[];
    ArrayBuild(I2C_req_buf, device_id, DEVICE_NAME);

    byte I2C_get_buf[];
    ArrayInit(I2C_get_buf, 0, 8);
    
    byte nbytes=8;
    char xstatus;

    while(LowspeedCheckStatus(DEVICE_PORT) == STAT_COMM_PENDING);
    LowspeedWrite(DEVICE_PORT, 8, I2C_req_buf);
    
    while(LowspeedStatus(DEVICE_PORT, nbytes) == STAT_COMM_PENDING);
    xstatus = LowspeedRead(DEVICE_PORT, nbytes, I2C_get_buf);
    Wait(100);

    if (xstatus == NO_ERR) return (ByteArrayToStr(I2C_get_buf));
    else                   return "Failed  " ;
 }

/* *******************************************************
 * I2C sensor initialization
 * ******************************************************* */
 void I2CSensor_Init()
 {
    SetSensorLowspeed(DEVICE_PORT);
    Wait(100);
 }

/* ********************************************************
 * main()
 * ******************************************************** */
 task main()
 {
     I2CSensor_Init();

     ClearScreen();
     TextOut(0, LCD_LINE1,"There are 3     " );
     TextOut(0, LCD_LINE2,"Digital sensors " );
     TextOut(0, LCD_LINE3,"connect to Port1" );
     TextOut(0, LCD_LINE8,"            Quit" );
     do
     {
          TextOut(0, LCD_LINE5, GetSensor(NXTHID_ID));
          TextOut(0, LCD_LINE6, GetSensor(US_ID));
          TextOut(0, LCD_LINE7, GetSensor(PFMATE_ID));
     }    until (ButtonPressed(BTNRIGHT, TRUE));

 }