void InitializeSonar(tSensors nPort) {
  static const byte kSonarInitialize[] = {3, 0x02, 0x41, 0x01};

  SensorType[nPort] = sensorI2CCustomStd9V;
  sendI2CMsg(nPort, kSonarInitialize[0], 0);
  wait10Msec(5);
}

void PingSonar(tSensors nPort) {
  static const byte kSonarPing[] = {3, 0x02, 0x41, 0x01};
  static const byte kSonarRead[] = {2, 0x02, 0x42};
  const int nSonarReplySize = 1;

  byte replyMsg[1];

  sendI2CMsg(nPort, kSonarPing[0], 0);
  wait1Msec(10); // wait 10ms, enough for echos to return to sensor.

  nI2CBytesReady[nPort] = 0; // Clear any pending bytes
  sendI2CMsg(nPort, kSonarRead[0], nSonarReplySize);
  while (nI2CStatus[nPort]==STAT_COMM_PENDING)
    wait1Msec(2); // Wait till I2C communication ends

  readI2CReply(nPort, replyMsg[0], nSonarReplySize);
  SensorValue[nPort] = replyMsg[0];
}

const tSensors kUS1 = S4;
const tSensors kUS2 = S3;

task main() {
	InitializeSonar(kUS1);
	InitializeSonar(kUS2);

	while (true) 	{
    PingSonar(kUS1);
	  PingSonar(kUS2);
    nxtDisplayTextLine(2, "US1: %d", SensorValue[kUS1]);
	  nxtDisplayTextLine(7, "US2: %d", SensorValue[kUS2]);
	  wait1Msec(50);
	}
	return;
}