ClearTimer(T1);

   do
   {
         StringFormat(TempStr, "temp : %04d \n",time1[T1]); writeDebugStream(TempStr);

     encLeft = HTANGreadAccumulatedAngle(angleLeft) * angleLeftRevert;
         StringFormat(TempStr, "temp : %04d \n",time1[T1]); writeDebugStream(TempStr); //9ms reading
     encRight = HTANGreadAccumulatedAngle(angleRight) * angleRightRevert;
         StringFormat(TempStr, "temp : %04d \n",time1[T1]); writeDebugStream(TempStr); //18ms reading
     //wait1Msec(1);
    if(rotation)
        error = encLeft+encRight;        // calculate the error by subtracting the offset
    else
        error = encLeft-encRight;        // calculate the error by adding the offset if robot turn

    StringFormat(TempStr2, "L %d R %d", encLeft, encRight);
    writeDebugStreamLine(TempStr2);
    integral = integral + error;        // calculate the integral
    integral=limite(-100,100,integral); // function limit value for integral between -100 and 100
    derivative = error - lastError;    // calculate the derivative
    Turn = (int)(Kp*(float)error + Ki*(float)integral + Kd*(float)derivative);  // the "P term" the "I term" and the "D term"
    powerRight = Tp + (int)Turn;                // the power level for the left motor
    powerLeft = Tp - (int)Turn;                // the power level for the right motor
....
   

bool writeI2C(tSensors link, tByteArray &data, int replylen) {

#if __COMMON_H_SENSOR_CHECK__ == 1
  TSensorTypes type = SensorType[link];
  if ((type != sensorI2CCustom) &&
      (type != sensorI2CCustom9V) &&
      (type != sensorI2CCustomFast) &&
      (type != sensorI2CCustomFast9V) &&
      (type != sensorI2CCustomFastSkipStates9V) &&
      (type != sensorI2CCustomFastSkipStates)) {
    hogCPU();
    PlaySound(soundException);
    eraseDisplay();
    nxtDisplayCenteredTextLine(0, "3rd Party Driver");
    nxtDisplayCenteredTextLine(1, "ERROR");
    nxtDisplayCenteredTextLine(2, "You have not");
    nxtDisplayCenteredTextLine(3, "setup the sensor");
    nxtDisplayCenteredTextLine(4, "port correctly. ");
    nxtDisplayCenteredTextLine(5, "Please refer to");
    nxtDisplayCenteredTextLine(6, "one of the");
    nxtDisplayCenteredTextLine(7, "examples.");
    wait1Msec(10000);
    StopAllTasks();
  }
#endif
writeDebugStreamLine("write");
ClearTimer(T2);
  if (!waitForI2CBus(link)) {
    clearI2CError(link, data.arr[1]);

    // Let's try the bus again, see if the above packets flushed it out
    // clearI2CBus(link);
    if (!waitForI2CBus(link))
      return false;
  }
writeDebugStreamLine("%d",time1[T2]);

  sendI2CMsg(link, data.arr[0], replylen);
writeDebugStreamLine("%d",time1[T2]);

  if (!waitForI2CBus(link)) {
    writeDebugStreamLine("error send");
    clearI2CError(link, data.arr[1]);
    sendI2CMsg(link, data.arr[0], replylen);
    if (!waitForI2CBus(link))
      return false;
  }
  writeDebugStreamLine("%d",time1[T2]);
  writeDebugStreamLine("end write");
  return true;
}

[color=#00BF00]--temp : 0000
write
T2 : 0000
T2 : 0000
T2 :  0004
end write
write
T2 : 0000
T2 : 0000
T2 :  0004
end write
write
T2 : 0000
T2 : 0000
T2 :  0006
end write
T4 : 0000
T4 : 0000
write
T2 : 0000
T2 : 0000
T2 :  0006
end write
T4 : 0000
T4 : 0000
write
T2 : 0000
T2 : 0000
T2 :  0006
end write
T4 : 0000
T4 : 0000
write
T2 : 0000
T2 : 0000
T2 :  0006
end write
T4 : 0000
T4 : 0000
--temp : 0632 [/color]
write
T2 : 0000
T2 : 0000
T2 :  0004
end write
write
T2 : 0000
T2 : 0000
T2 :  0007
end write
T4 : 0000
T4 : 0000
--temp : 0012
write
T2 : 0000
T2 : 0000
T2 :  0007
end write
T4 : 0000
T4 : 0000
--temp : 0008
write
T2 : 0000
T2 : 0001
T2 :  0007
end write
T3 : 0008
T4 : 0000
T4 : 0000
T3 : 0000
--temp : 0008
write
T2 : 0001
T2 : 0000
T2 :  0007
end write
T3 : 0008
T4 : 0000
T4 : 0000
T3 : 0000
--temp : 0008
write
T2 : 0000
T2 : 0000
T2 :  0007
end write
T4 : 0000
T4 : 0000
--temp : 0008
[color=#FF0000]value : 0308,0308
0012,0012,0240[/color]

#pragma config(Sensor, S1,     ANGLE,               sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

ubyte xmit_msg[] = {2, 0x02, 0x42};
ubyte recv_msg[4];

task main () {
  long time = 0;
  time1[T1] = 0;
  for (int i = 0; i < 100; i++) {
    while(nI2CStatus(ANGLE) != NO_ERR) EndTimeSlice();
    sendI2CMsg(ANGLE, xmit_msg[0], 4);
    while(nI2CStatus(ANGLE) != NO_ERR) EndTimeSlice();
    readI2CReply(ANGLE, recv_msg[0], 4);
  }
  time = time1[T1];
  writeDebugStreamLine("Each call: %d", time/100);
  writeDebugStreamLine("Total time: %d", time);
}

#pragma config(Sensor, S1,     ANGLE1,              sensorI2CCustom)
#pragma config(Sensor, S2,     ANGLE2,              sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

ubyte xmit_msg[] = {2, 0x02, 0x42};
ubyte recv_msg[4];

task main () {
  long time = 0;
  time1[T1] = 0;
  for (int i = 0; i < 100; i++) {
    while((nI2CStatus(ANGLE1) != NO_ERR) && (nI2CStatus(ANGLE2) != NO_ERR)) EndTimeSlice();
    sendI2CMsg(ANGLE1, xmit_msg[0], 4);
    sendI2CMsg(ANGLE2, xmit_msg[0], 4);
    while((nI2CStatus(ANGLE1) != NO_ERR) && (nI2CStatus(ANGLE2) != NO_ERR)) EndTimeSlice();
    readI2CReply(ANGLE1, recv_msg[0], 4);
    readI2CReply(ANGLE2, recv_msg[0], 4);
  }
  time = time1[T1];
  writeDebugStreamLine("Each call: %d", time/100);
  writeDebugStreamLine("Total time: %d", time);
}