Updated TeensyWay test code to be compatibel with i2c_t3