Big Brother is watching you

1/********** Big Brother **********/
2/******** is watching you ********/
3
4/* Adjust these two values to reflect the
5   position of the X ultrasonic sensor. */
6const int A=20; // cm
7const int B=15; // cm
8
9/* Hardware connections */
10
11const int trigPinX=2, echoPinX=3;
12const int trigPinY=5, echoPinY=6;
13const int servoPinL=8;
14const int servoPinR=10;
15
16float measureDist(int trigPin, int echoPin)
17{
18  digitalWrite(trigPin, HIGH);
19  delayMicroseconds(20);
20  digitalWrite(trigPin, LOW);
21  return pulseIn(echoPin, HIGH)/2*.0343;  
22}
23
24void servoControl(int servoPin, int angle)
25{
26  unsigned int duration;
27
28  duration=map(angle, 0, 180, 544, 2400);
29  digitalWrite(servoPin, HIGH);
30  delayMicroseconds(duration);
31  digitalWrite(servoPin, LOW);
32}
33
34void setup()
35{
36  pinMode(trigPinX, OUTPUT);
37  pinMode(trigPinY, OUTPUT);
38  pinMode(servoPinL, OUTPUT);
39  pinMode(servoPinR, OUTPUT);
40}
41
42void loop()
43{
44  float distX, distY;
45  int pos, diff;
46  int limDistX, limDistY;
47
48  distX=measureDist(trigPinX, echoPinX);
49  limDistX=(int)constrain(distX, 4, 2*A);
50  pos=map(limDistX, 4, 2*A, 45, 135);
51
52  distY=measureDist(trigPinY, echoPinY);
53  limDistY=(int)constrain(distY, 4, B);
54  diff=map(limDistY, 4, B, 40, 0);
55
56  if(pos==135)
57  {
58    pos=90;
59    diff=0;
60  }
61
62  servoControl(servoPinL, pos-diff);
63  servoControl(servoPinR, pos+diff);
64  delay(60);
65}