I2C Selectable by function

[Sandroelec01]

Hello,

I wrote a function that need to select the I2C port to write on. I paste here the code

int readRegisterAD(uint8_t IMUPosition, uint8_t ADDRESS, uint8_t reg, uint8_t lunghezza) {
  TwoWire* WirePort = &Wire;
  if ((IMUPosition == 0x00) || (IMUPosition == 0x01)) {
    WirePort = &Wire;
  }
  if ((IMUPosition == 0x02) || (IMUPosition == 0x03)) {
    WirePort = &Wire1;
  }
  if ((IMUPosition == 0x04) || (IMUPosition == 0x05)) {
    WirePort = &Wire2;
  }
  WirePort->beginTransmission(ADDRESS);
  WirePort->write(reg);
  if (WirePort->endTransmission(true) != 0) {
#if DEBUG
    Serial.println("Error writing device location ReadingRegister");
#endif
  }
  WirePort->beginTransmission(ADDRESS);
  WirePort->requestFrom(ADDRESS, lunghezza);
  int response = readWire(IMUPosition);
  if (WirePort->endTransmission() != 0) {
#if DEBUG
    Serial.println("Error Req device location ReadingRegister");
#endif
  }
  return response;
}

I'm working with Teensy 4.1 and Arduino IDE (TeensyDuino installed) Unfortunately it doesn't work. I put scilloscope probe on Wire1 and WIre2 Ports selecting the right port but I can work only with the normal Wire Wire1 and 2 are not alive and not selected.
I wrote same code specifyng wire port 1 and 2 and the hardware it's ok, my only problem it's the function that select ports.
Anyone can help?
Thanks a lot

Sandro

 

[Sandroelec01]

Problem solved I was wrong calling the function