BlueStamp-TabletopRobot Final

gistfile1.txt

//Include and create objects within libraries
#include <Servo.h>
#include <SoftwareSerial.h>
Servo servo1;
Servo servo2;
SoftwareSerial XBee(2, 3);

//Declare pin references & other constants
int servo1Pin = 9;
int servo2Pin = 10;
int ledPin = 8;
int IRemitter1Pin = 11;
int IRemitter2Pin = 12;
int IRdetector1Pin = 6;
int IRdetector2Pin = 7;
int PULSE = 38000;
int SERVO_SPEED = 10;
int CYCLE = 1000;
String lastSent;
int XBEE_SEND_FREQ = 200;
int IR_CHECK_FREQ = 40;
double pie = 3.141592653589792;

double direct = 0;
double x = 0;
double y = 0;
void setup() {
  Serial.begin(9600);
  XBee.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 < CYCLE; i++){  //Using a for loop instead of delay() so that I can run other code at the same time.
    if(i % IR_CHECK_FREQ == 0){  // Run for every IR_CHECK_FREQ because the sensor cannot handle long duration of IR light.
      IRcheck(IRemitter1Pin, IRdetector1Pin, servo1, servo2, 1);   //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, -1);   //Same thing but for the second pair.
    }
    if(i % XBEE_SEND_FREQ == 0){
      //Serial.print((int)(x * 100));
        XBee.print ((int)(y / 100));
        XBee.print(",");
        XBee.print((int)(x / 100));
        XBee.print(".");
//        XBee.print(direct);
//        XBee.println(".");
    }
    
    if (XBee.available()){
      remoteCheck(servo2, servo1, XBee.read());
    }
    x += cos(direct / 180 * pie) * SERVO_SPEED;
    y += sin(direct / 180 * pie) * SERVO_SPEED;
    delay(1);  //Wait one mS to record time for the loop 
    /*
    Serial.print("Servo1:");
    Serial.println((int)servo1.read());
    Serial.print("Servo2:");
    Serial.println((int)servo2.read());
  */
  }
}


void remoteCheck(Servo servoLeft,Servo servoRight, char c){          
      switch (c){
        case 'w':      // If received 'w'
        case 'W':      // or 'W'
          servoLeft.write(90 + SERVO_SPEED);
          servoRight.write(90 - SERVO_SPEED);
          if(direct > 359){
            direct -= 360;
          }
          break;
          
        case 'a':      // If received 'a'
        case 'A':      // or 'A'b
          turn(servoLeft, servoRight, -1);
          break;
        case 's':      // If received 's'
        case 'S':      // or 'S'
          servoLeft.write(90 - SERVO_SPEED);
          servoRight.write(90 + SERVO_SPEED);
          direct += 180;
          if(direct > 359){
            direct -= 360;
          }
          break;
        case 'd':      // If received 'd'
        case 'D':      // or 'D'
          turn(servoRight, servoLeft, 1);
          break;
      }
}

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

void turn(Servo backServo, Servo otherServo, int turnDirect){
  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.
  direct = (direct + SERVO_SPEED * rdm / 138 * turnDirect) / 360;
  if(direct > 360) direct -= 360;
  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);
}