BlueStamp-TabletopRobot

#include <Servo.h>

//Declare servos
Servo servo1;
Servo servo2;\

//Declare pin references & other constants
int servo1Pin = 9;
int servo2Pin = 10;
int ledPin = 8;
int IRemitter1Pin = 11;
int IRemitter2Pin = 12;
int IRdetector1Pin = 7;
int IRdetector2Pin = 6;

int PULSE = 38000;
int SERVO_SPEED = 5;
int IR_CHECK_FREQ = 40;

void setup() {
    Serial.begin(9600);
  pinMode(servo1Pin, OUTPUT);
  pinMode(servo2Pin, OUTPUT);
  pinMode(IRdetector1Pin, INPUT);
  pinMode(IRdetector2Pin, INPUT);
  pinMode(ledPin, OUTPUT); 
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  randomSeed(analogRead(0));
  servo1.write(90 - SERVO_SPEED);
  servo2.write(90 + SERVO_SPEED);
}

void loop(){  
  for(int i = 0; i < IR_CHECK_FREQ; i++){  //Using a for loop instead of delay() so that I can run other code at the same time.
    if(i == IR_CHECK_FREQ - 1){  // Run for every IR_CHECK_FREQ because the sensor cannot handle long duration of IR light.
      IRcheck(IRemitter1Pin, IRdetector1Pin, servo1, servo2);   //Check 1st pair of IR and go backward a bit if the sensor does not detect the IR reflected from table.
      IRcheck(IRemitter2Pin, IRdetector2Pin, servo2, servo1);   //Same thing but for the second pair.
    }    
    delay(1);  //Wait one mS to record time for the loop.
  }
  
}

void IRcheck(int emitter, int detector, Servo servo, Servo otherServo){
    tone(emitter, PULSE);  //Tone the emitter to the frequency the detector can detect.                            
    delay(1);  //Give time for detector to react;
    if(digitalRead(detector)) reactToEdge(servo, otherServo);  //If not detect IR reflected from the table, Go back for a random amount of time.
    noTone(emitter);   //Stop tonning to give detector a rest and let the other pair check IR.
}

void reactToEdge(Servo backServo, Servo otherServo){
  int ogBackServoSpeed = (int)backServo.read(); //Save the original speed of servos.
  int ogOtherServoSpeed = (int)otherServo.read();
  otherServo.write(90);  //Stop the not related servo from moving.
  backServo.write(180 - ogBackServoSpeed);   //Turn the servo to rotate in the opposite direction;
  int rdm = random(40, 1000);    //Generate a random number.
  delay(rdm);  //Keep the servo moving the other direction for a random amount of time
  otherServo.write(ogOtherServoSpeed);  //Load back the original speed of servos.
  backServo.write(ogBackServoSpeed);
}