byte CMDNunchukInit[] = {0x03, 0xA4, 0x40, 0x00};
byte CMDNunchukWrite[] = {0x02, 0xA4, 0x00};
byte CMDNunchukRead[] = {0x01, 0xA4};

byte outbuf[6];
int outbuf1[6];   //ubyte workaround for ROBOTC

//  This function allows conversion of an unsigned byte to a signed int
//  This is a workaround for ROBOTC
int ubyteToInt(ubyte _byte) {
   int _ret = 0;
   _ret = (_byte & 0x80) ? (_byte & 0x7F) + 0x80 : _byte;
   return _ret;
}

// port is the NXT port, i2c_cmd is the nunchuk internal address register
// at i2c_cmd=0...5 are 6 Byte of sensor values
// at i2c_cmd=0x20 ... 0x2F  you get 16 Byte of calibration data

sub NunchukGetParamter(tSensors port, byte i2c_cmd)
{
  // Initialize the Nunchuk
   SensorType[port] = sensorI2CCustom;

  //Ask for 0 bytes
   sendI2CMsg(port, CMDNunchukInit[0], 0);

  while (nI2CStatus[port] == STAT_COMM_PENDING) ; // ROBOTC Wait for I2C bus to be ready

  //Write at 0x00: 6 Bytes of sensor values
  CMDNunchukWrite[2] =i2c_cmd;

  //Write Nunchuk register address
   sendI2CMsg(port, CMDNunchukWrite[0], 0);

  while (nI2CStatus[port] == STAT_COMM_PENDING) ; // ROBOTC Wait for I2C bus to be ready

  byte count = 6;
  //Ask for count bytes
   sendI2CMsg(port, CMDNunchukRead[0], count);

  while (nI2CStatus[port] == STAT_COMM_PENDING) ; // Wait for I2C bus to be ready

  if(nI2CBytesReady[port] == count){

    //Read data from buffer
     readI2CReply(port, outbuf[0], 6);

    for(int i=0; i<count; i++ ){
       outbuf1[i]=ubyteToInt(outbuf[i]);   //ubyte workaround for ROBOTC
      outbuf1[i]=(outbuf1[i]^0x17) + 0x17;
    }
  }
  else {
    // error
     memset(outbuf, 0, 0);
  }
}