Arduino.ru

Всем привет. Пытаюсь сделать автоматический поиск станции зарядки на 2-х Irda приемниках и одном Irda светодиоде. Возникли сложности в реализации. Сейчас в качестве вещания станция использует протокол NEC. Условно говоря передает каждые 100мс сигнал с определенным кодом клавиши, а робот принимает его. Возникла сложность в реализации алгоритма. Работает не очень точно и куча проблем. К примеру робот слишком сильно поворачивает в нужном направлении чтобы оба приемника видели сигнал и из-за этого происходит дергание(то в лево то в право), в итоге получается какая-то фигня. Как упростить все и сделать как потолежно? Пожалуйста подскажите. Спасибо!

Вот мой код(набросок):

#define IRDA_RIGHT_PIN 11
#define IRDA_LEFT_PIN 12
#define LED 13

#define LEFT_ENGINES_PIN 7
#define LEFT_SPEED_PIN 10
#define RIGHT_ENGINES_PIN 8
#define RIGHT_SPEED_PIN 9

long previousMillis = 0;

long rightCode = 0;
long leftCode = 0;

//0xFF18E7 - код который передает станция.

void setup() {
  Serial.begin(9600);
  pinMode(IRDA_RIGHT_PIN, INPUT);
  pinMode(IRDA_LEFT_PIN, INPUT);
  pinMode(LED, OUTPUT);
}

void loop() {

    rightCode = scanEye(IRDA_RIGHT_PIN,200);
    leftCode = scanEye(IRDA_LEFT_PIN,200);
  
   if(rightCode!=0 && leftCode==0){
       Serial.println("To right...");
        controlEngines('L',0,100);
        controlEngines('R',1,100);
    }else if(rightCode==0 && leftCode!=0){
        Serial.println("To left...");
        controlEngines('L',1,100);
        controlEngines('R',0,100);
    }else if((rightCode==leftCode) && (rightCode!=0 && leftCode!=0)){
        Serial.println("To forward!");
        controlEngines('L',0,100);
        controlEngines('R',0,100);
    }else if(rightCode==0 && leftCode==0){
        Serial.println("Stop!");
        controlEngines('L',0,0);
        controlEngines('R',0,0);
    }

}

long scanEye(int eye, int ms) {
  long code = 0;
  while (millis() - previousMillis < ms) {
    code = readFromIrda(eye);
  }
  previousMillis = millis();
  return code;
}

void controlEngines(byte engine, byte engineDirection, byte engineSpeed) {
  switch (engine)
  {
    case 'L':
      digitalWrite(LEFT_ENGINES_PIN, engineDirection);
      analogWrite(LEFT_SPEED_PIN, engineDirection == 0 ? engineSpeed : 255 - engineSpeed);
      break;
    case 'R':
      digitalWrite(RIGHT_ENGINES_PIN, engineDirection);
      analogWrite(RIGHT_SPEED_PIN, engineDirection == 0 ? engineSpeed : 255 - engineSpeed);
      break;
  }
}


long readFromIrda(byte irdaPin) {
  byte irCode[32];
  unsigned long starttime = 0;
  unsigned long endtime = 0;
  unsigned long time = 0;

  //Если есть сигнал
  if (digitalRead(irdaPin) == LOW) {
    starttime = micros(); //Запоминаем начало сигнала
    while (digitalRead(irdaPin) == LOW); //Ждем пока сигнал закончится
    endtime = micros(); //Время окончания сигнала
    time = endtime - starttime; //Продолжительноть сигнала

    //Если сигнал длился около 9мс, то скорее всего это стартовый бит
    if (time > 8000) {
      int i = 0; //Нам нужно считать 32 бита
      while (true) {
        //Если сигнал отсуствует
        if (digitalRead(irdaPin) == HIGH) {

          starttime = micros(); //Запоминаем время начала отсутсвия сигнала
          while (digitalRead(irdaPin) == HIGH); //Ждем пока сигнала нет
          endtime = micros(); //Сигнал появился
          time = endtime - starttime; //Высчитываем время отсутсвия сигнала

          //Пропускаем паузу перед началом данных. Около 4.5мс(протокол NEC)
          if (time < 4000) {
            irCode[i] = time > 1000 ? 1 : 0; //Если сигнал отсуствовал более 1мс, то это логическая 1, иначе 0
            i++;//Увеличиваем счетчик принятых битов
          }

          /*
          Все 32 бита приняты!
          Делаем задержку для пропуска ненужных данных. Примерно 100мс
          */
          if (i > 31) {
            delay(10);
            return decodeIRCode(irCode);
          }
        }
      }
    }
  }
  return 0;
}

long _readFromIrda(byte irdaPin) {
  byte irCode[32];
  int time = pulseIn(irdaPin, LOW,10000);;

  //Если сигнал длился около 9мс, то скорее всего это стартовый бит
  if (time > 8000) {
    int i = 0; //Нам нужно считать 32 бита
    while (true) {
      time = pulseIn(irdaPin, HIGH,5000);

      if (time > 2000) {
        return 0;
      }

      if (i > 31) {
        return decodeIRCode(irCode);
      }
      
      irCode[i] = time > 900 ? 1 : 0;
      i++;
    }
  }

  return 0;
}


long decodeIRCode(byte *irCode) {
  //Преобразовываем.
  byte data[4];
  toBytes(irCode, data);
  long code = bytesToLong(data);
  return code;
  //
}

long bytesToLong(byte* data) {
  long val = 0;
  val += data[0] << 24;
  val += data[1] << 16;
  val += data[2] << 8;
  val += data[3];
  return val;
}

//ОПТИМИЗИРОВАТЬ!
void toBytes(byte *mass, byte *data) {
  data[0] = mass[7] * 128 + mass[6] * 64 + mass[5] * 32 + mass[4] * 16 + mass[3] * 8 + mass[2] * 4 + mass[1] * 2 + mass[0];
  data[1] = mass[15] * 128 + mass[14] * 64 + mass[13] * 32 + mass[12] * 16 + mass[11] * 8 + mass[10] * 4 + mass[9] * 2 + mass[8];
  data[2] = mass[23] * 128 + mass[22] * 64 + mass[21] * 32 + mass[20] * 16 + mass[19] * 8 + mass[18] * 4 + mass[17] * 2 + mass[16];
  data[3] = mass[31] * 128 + mass[30] * 64 + mass[29] * 32 + mass[28] * 16 + mass[27] * 8 + mass[26] * 4 + mass[25] * 2 + mass[24];
}