NXTViSy.nxc

#define I2C_ADDRESS 0x14
#define GET_DATA 0x01
#define SET_SIGNATURE 0x02
#define SET_HEIGHT 0x04
#define SET_CALIB_CONST 0x08
#define GET_DATA_I2C_BYTES_ERR 0x01
#define GET_DATA_RECEIVED_BYTES_ERR 0x02

byte NXTViSy_port;

void SetSensorNXTViSy(byte port)
{
    NXTViSy_port = port;
    SetSensorLowspeed(port);
}
byte NXTViSyGetData(int &distance, byte &action)
{
    byte received_bytes_count = 0x03;
    byte inBuffer[2] = { I2C_ADDRESS, GET_DATA };
    byte outBuffer[];
    long error;
    
    while (I2CCheckStatus(NXTViSy_port) != NO_ERR)
        ;
    if (I2CBytes(NXTViSy_port, inBuffer, received_bytes_count, outBuffer) == false)
        return GET_DATA_I2C_BYTES_ERR;
    if (received_bytes_count != 0x03)
        return GET_DATA_RECEIVED_BYTES_ERR;

    distance = outBuffer[0];
    distance |= outBuffer[1] << 8;
    action = outBuffer[2];
    
    return NO_ERR;
}
long NXTViSyInit(byte sig, byte height, unsigned int calib_dis_pix)
{
    byte inBuffer[6];
    
    inBuffer[0] = I2C_ADDRESS;
    inBuffer[1] = SET_SIGNATURE | SET_HEIGHT | SET_CALIB_CONST;
    inBuffer[2] = sig;
    inBuffer[3] = height;
    inBuffer[4] = calib_dis_pix & 0xFF; // low bits
    inBuffer[5] = calib_dis_pix >> 8; // high bits
    
    while (I2CCheckStatus(NXTViSy_port) != NO_ERR)
        ;
    return I2CWrite(NXTViSy_port, 0, inBuffer);
}