1

I have a teensy that controls a motor and reads from a ir sensor, GP2Y0E02b. The issue i run into is that at random times the teensy seems to freeze, the main code stops printing but the interrupt routine continue to work. It seems to be that when reading the ir sensor it causes the teensy to freeze. The code below is what I'm using to read the ir values.

#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;
  }

};

0 Answers0