Huge delays and problems with hardware/software

Hello,

I’m trying to make a boebot which will follow light on 2 levels in height. Both use phototransistors, the bottom one has priority over the top one. So if I aim a light source on the top sensor, it should react to it, but when I aim another light source at the bottom sensor, it should ignore the top sensor. Besides that, I want to use a parallax ping sensor to let it do other actions depending on the distance.

The code should theoretically work fine (may not be complete yet), but it has an extreme delay when I enable ParallaxPing();. Without it, the LED I use for the bottom sensor “foto2”, still flickers, but at a fast rate (same speed as the LED on the Arduino (uno).
The ping sensor doesn’t even report any distance and its LED doesn’t blink (The code for it is correctly made and tested).
I made the servos work without libraries (I don’t want to use any library).

Here is my code:

#define ServoL 10       // Linker Servo
#define ServoR 11       // Rechter Servo
int DutyL = 500;
int DutyR = 500;

//IR Sensor
int IRLinks;
int IRRechts;

//Laser
int foto0;
int foto1;
int foto2;
long grenswaarde = 350;
boolean laser = false;

//Ping Sensor
int signal = 12;
int afstand;
unsigned long pulse = 0;

void setup() {
  Serial.begin(250000);

  //IR Sensor
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);

  //Laser
  pinMode(A1, INPUT);
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);

  //Ping Sensor
  pinMode(signal, OUTPUT);

  //Servo's
  pinMode(ServoL, OUTPUT);
  pinMode(ServoR, OUTPUT);

  // initialize Timer1
  cli();          // disable global interrupts
  TCCR1A = 0;     // set entire TCCR1A register to 0
  TCCR1B = 0;     // same for TCCR1B

  // set compare match register to desired timer count:
  OCR1A = 312;
  // turn on CTC mode:
  TCCR1B |= (1 << WGM12);
  // Set CS11 bits for 1024 prescaler:
  TCCR1B |= (1 << CS10);
  TCCR1B |= (1 << CS12);
  // enable timer compare interrupt:
  TIMSK1 |= (1 << OCIE1A);
  // enable global interrupts:
  sei();
}

void loop() {
  ParallaxPing();

  if (laser == false) {
    IR();
    Laser();

    if (IRLinks <= 15 && IRRechts >= 15) {
      Links();
    }
    if (IRLinks <= 15 && IRRechts >= 15 && afstand <= 10) {
      Links2();
    }
    if (IRRechts <= 15 && IRLinks >= 15) {
      Rechts();
    }
    if (IRRechts <= 15 && IRLinks >= 15 && afstand <= 10) {
      Rechts2();
    }
    if (IRLinks <= 15 && IRRechts <= 15 && afstand >= 12) {
      Vooruit();
    }
    if (IRRechts > 20 && IRLinks > 20) {
      Draai();
    }
    if (afstand <= 10) {
      Achteruit();
    }
  }

  if (laser == true) {
    Laser2();
  }

  Print();
}

void IR() {
  IRLinks = analogRead(A3);
  IRRechts = analogRead(A4);
}

void Vooruit() {
  //Rijd rechtdoor
  DutyL = 700;
  DutyR = 300;
}

void Achteruit() {
  //Rijd achteruit
  DutyL = 300;
  DutyR = 700;
}

void Links() {
  //Stuur naar links
  DutyL = 550;
  DutyR = 300;
}

void Links2() {
  //Draai naar links
  DutyL = 300;
  DutyR = 300;
}

void Rechts() {
  //Stuur naar rechts
  DutyL = 700;
  DutyR = 450;
}

void Rechts2() {
  //Draai naar rechts
  DutyL = 700;
  DutyR = 700;
}

void Draai() {
  //Draai naar rechts
  DutyL = 700;
  DutyR = 700;
}


void Laser() {
  foto0 = analogRead(A0);
  foto1 = analogRead(A1);
  foto2 = analogRead(A2);
  if (foto0 >= grenswaarde || foto1 >= grenswaarde || foto2 >= grenswaarde) {
    laser = true;
    Serial.println("T");
  }
  else {
    foto0 = 0;
    foto1 = 0;
    foto2 = 0;
    digitalWrite(2, HIGH);
    digitalWrite(3, LOW);
    digitalWrite(4, HIGH);
    digitalWrite(5, LOW);
    digitalWrite(6, HIGH);
    digitalWrite(7, LOW);
  }
}

void Laser2() {
  foto0 = analogRead(A0);
  foto1 = analogRead(A1);
  foto2 = analogRead(A2);
  if (foto0 >= grenswaarde) { //Sensor rechts
    digitalWrite(7, HIGH);
    digitalWrite(6, LOW);

    DutyL = 500;
    DutyR = 500;
  }
  else {
    digitalWrite(6, HIGH);
    digitalWrite(7, LOW);
  }
  if (foto1 >= grenswaarde) { //Sensor achter
    digitalWrite(5, HIGH);
    digitalWrite(4, LOW);

    DutyL = 500;
    DutyR = 500;
  }
  else {
    digitalWrite(4, HIGH);
    digitalWrite(5, LOW);
  }
  if (foto2 >= grenswaarde) { //Sensor links
    digitalWrite(3, HIGH);
    digitalWrite(2, LOW);

    DutyL = 500;
    DutyR = 500;
  }
  else {
    digitalWrite(2, HIGH);
    digitalWrite(3, LOW);
  }
}

void ParallaxPing() {
  pinMode(signal, OUTPUT);

  digitalWrite(signal, LOW);
  delayMicroseconds(50);

  digitalWrite(signal, HIGH);
  delayMicroseconds(50);
  digitalWrite(signal, LOW);

  pinMode(signal, INPUT);

  pulse = pulseIn(signal, HIGH);

  pulse = pulse / 2;
  afstand = int(pulse / 29);
}

ISR(TIMER1_COMPA_vect) {
  digitalWrite(ServoL, HIGH);
  delayMicroseconds(DutyL);
  digitalWrite(ServoL, LOW);

  delayMicroseconds(2000 - DutyL);
  digitalWrite(ServoR, HIGH);
  delayMicroseconds(DutyR);
  digitalWrite(ServoR, LOW);
}

void Print() {
  Serial.print("IR Links - ");
  Serial.println(IRLinks);
  Serial.print("IR Rechts - ");
  Serial.println(IRRechts);
  Serial.print("\n");

  Serial.print("Laser Links - ");
  Serial.println(foto2);
  Serial.print("Laser Rechts - ");
  Serial.println(foto0);
  Serial.print("Laser Achter - ");
  Serial.println(foto1);
  Serial.print("\n");

  Serial.print("Afstand - ");
  Serial.println(afstand);
  Serial.println("________________________\n");
}

Edit: Code updated, fixed small mistakes

The ping sensor doesn't even report any distance

. . . so the pulseIn is probably timing-out.

AWOL:
. . . so the pulseIn is probably timing-out.

Okay, it turns out that the cables I used were faulty. I replaced them with original cables and it reports.
I found out that I was using pin 10 and 11 for visual leds while it should be for the servo motors. This is going to be fixed.

I found out that when the distance measured is less, it posts quicker on the serial monitor.

I found out that when the distance measured is less, it posts quicker on the serial monitor.

Which may not not be a Good Thing TM.

If you ping too frequently, you run the risk of mistaking late-arriving echoes from the previous ping for nearby echoes from the latest ping…

AWOL:
Which may not not be a Good Thing TM.

If you ping too frequently, you run the risk of mistaking late-arriving echoes from the previous ping for nearby echoes from the latest ping…

This is the ping code:

void ParallaxPing() {
  pinMode(signal, OUTPUT);

  digitalWrite(signal, LOW);
  delayMicroseconds(50);

  digitalWrite(signal, HIGH);
  delayMicroseconds(50);
  digitalWrite(signal, LOW);

  pinMode(signal, INPUT);

  pulse = pulseIn(signal, HIGH);

  pulse = pulse / 2;
  afstand = int(pulse / 29);
}

It pings with 50ms delay, should be fine, right?

I don't want to use any library

Better get rid of this line, then:

  Serial.begin(250000);