Mucho frío al leer el sensor de infrarrojos

Tengo un modelo que controla un motor y lee un sensor IR, GP2Y0E02b. El problema con el que me encuentro es que, en momentos aleatorios, parece que la adolescencia se congela, el código principal deja de imprimir, pero la rutina de interrupción continúa funcionando. Parece ser que cuando se lee el sensor de infrarrojos, el adolescente se congela. El código a continuación es lo que estoy usando para leer los valores de IR.

#include <i2c_t3.h>
//#include <WSWire.h>
#include <Arduino.h>
uint16_t ReadFirstSensor();
uint16_t ReadSecondSensor();

#define ADDRESS       0x80 >> 1  // Arduino uses 7 bit addressing so we shift 
address right one bit
#define DISTANCE_REG  0x5E
#define SHIFT         0x35 

class GP2Y0E02B {
public:
    int MaxDistance;
   int MinDistance = 27;
float distance = 0;              
byte high, low = 0;              
float shift = 0; 
bool badSensor = false;

GP2Y0E02B(){
  //Serial.println("Sensors Initialized");
};

float GetRawDistance()
{
    //if (!badSensor){
      float FirstDistance = ReadFirstSensor(); 
      return FirstDistance; 
    //}
    //else 
    //  return 22000;
}

void StartSensors()
{
    //Serial.println("StartSensors Called");
    Wire.begin(400000);
    delay(50);
    //Serial.println("Wire.begin() has been called");
    Wire.beginTransmission(ADDRESS);
    //Serial.println ("beginTrans done");
    Wire.write(SHIFT);
    //Serial.println ("Shiftwrite done");
    int stat = Wire.endTransmission();
    //Serial.print("endTrans done stat: ");Serial.println(stat);

    int i = 0; 
    while (Wire.available() == 0)
    {
        //Serial.println ("Im trying SPI");
        i++;
        delay(5);
        if (i>30) break;
    }
    if (i<30)
    {
        shift = Wire.read();
        Wire.requestFrom(ADDRESS, 1);
        MaxDistance = GetRawDistance();
        badSensor = false;
    }
    else 
    {
        //Serial.println ("couldnt initialize the sensors");
        badSensor = true;
    }
 }

 uint16_t ReadFirstSensor() 
 {
    static int LastDist = 0;
    //Serial.println ("Read First sensor called");
    Wire.beginTransmission(ADDRESS);
    Wire.write(DISTANCE_REG);
    //Serial.println ("DoneWireWrite");
    int SPIhealth = Wire.endTransmission();
    //Serial.print ("DoneWireEndTransmit: ");Serial.println (SPIhealth);
    if (SPIhealth == 0) Wire.requestFrom(ADDRESS, 2);
    else return LastDist;
    //Serial.println ("DoneWireREquest");

    int EmergBreak = 0;
    while(Wire.available() < 2)
    {
        //Serial.println ("Trying to get sensor reading");
        EmergBreak++;
        delay(1);
        if (EmergBreak > 20) return LastDist;
    }
    if (EmergBreak > 20);//Serial.println ("Couldnt Read Sensor");
    //else Serial.println ("Done Reading Sensor");

    high = Wire.read();
    low = Wire.read();

    uint16_t rawDistance; 
    rawDistance = (high * 16 + low)/16/(float)pow(2,shift); // Calculate the range in CM 
    //Serial.print("rawDistance: ");Serial.println(rawDistance);
    rawDistance = (uint16_t) low ;
    rawDistance += (uint16_t)high << 8;
    LastDist = rawDistance;

    return rawDistance;
  }

};