Pan Tilt Processing Sketch

/**********************************************************************************************
* Pan/Tilt Face Tracking Sketch
* Written by Ryan Owens for SparkFun Electronics
* Uses the OpenCV real-time computer vision  framework from Intel
* Based on the OpenCV Processing Examples from ubaa.net
* This example is released under the Beerware License.
* (Use the code however you'd like, but mention us and by me a beer if we ever meet!)
*
* The Pan/Tilt Face Tracking Sketch interfaces with an Arduino Main board to control
* two servos, pan and tilt, which are connected to a webcam. The OpenCV library
* looks for a face in the image from the webcam. If a face is detected the sketch
* uses the coordinates of the face to manipulate the pan and tilt servos to move the webcam
* in order to keep the face in the center of the frame.
*
* Setup-
* A webcam must be connected to the computer.
* An Arduino must be connected to the computer. Note the port which the Arduino is connected on.
* The Arduino must be loaded with the ServoFirmata Sketch.
* Two servos mounted on a pan/tilt backet must be connected to the Arduino pins 4 and 7.
* The Arduino must be powered by a 9V external power supply.
*
* Read this tutorial for more information:
**********************************************************************************************/
import gab.opencv.*;
import processing.video.*;
import java.awt.*;
import processing.serial.*;

Capture video;
OpenCV opencv;  //Create an instance of the OpenCV library.

Serial serial; // The serial port

//Variables for keeping track of the current servo positions.
char servoTiltPosition;
char servoPanPosition;
//The pan/tilt servo ids for the Arduino serial command interface.
char tiltChannel = 0;
char panChannel = 1;

//These variables hold the x and y location for the middle of the detected face.
int midFaceY=0;
int midFaceX=0;

//The variables correspond to the middle of the screen, and will be compared to the midFace values
int midScreenY = (height/2);
int midScreenX = (width/2);
int midScreenWindow = 10;  //This is the acceptable 'error' for the center of the screen.

//The degree of change that will be applied to the servo each time we update the position.
int stepSize=1;

void setup() {
  size(640, 480);
  video = new Capture(this, 640/2, 480/2);
  opencv = new OpenCV(this, 640/2, 480/2);
  opencv.loadCascade(OpenCV.CASCADE_FRONTALFACE);

  video.start();

  printArray(Serial.list());


  serial = new Serial(this, "COM3", 57600);

}



void draw() {
  scale(2);
  opencv.loadImage(video);

  image(video, 0, 0 );

  noFill();
  stroke(53, 204, 255);
  strokeWeight(3);
  Rectangle[] faces = opencv.detect();
  println(faces.length);

  for (int i = 0; i < faces.length; i++) {
    println(faces[i].x + "," + faces[i].y);
    rect(faces[i].x, faces[i].y, faces[i].width, faces[i].height);
  }


  if(faces.length > 0){
    //If a face was found, find the midpoint of the first face in the frame.
    //NOTE: The .x and .y of the face rectangle corresponds to the upper left corner of the rectangle,
    //      so we manipulate these values to find the midpoint of the rectangle.
    midFaceY = faces[0].y + (faces[0].height/2);
    midFaceX = faces[0].x + (faces[0].width/2);

    //Find out if the Y component of the face is below the middle of the screen.
    if(midFaceY < (midScreenY - midScreenWindow)){
      if(servoTiltPosition >= 5)servoTiltPosition -= stepSize; //If it is below the middle of the screen, update the tilt position variable to lower the tilt servo.
    }
    //Find out if the Y component of the face is above the middle of the screen.
    else if(midFaceY > (midScreenY + midScreenWindow)){
      if(servoTiltPosition <= 175)servoTiltPosition +=stepSize; //Update the tilt position variable to raise the tilt servo.
    }
    //Find out if the X component of the face is to the left of the middle of the screen.
    if(midFaceX < (midScreenX - midScreenWindow)){
      if(servoPanPosition >= 5)servoPanPosition -= stepSize; //Update the pan position variable to move the servo to the left.
    }
    //Find out if the X component of the face is to the right of the middle of the screen.
    else if(midFaceX > (midScreenX + midScreenWindow)){
      if(servoPanPosition <= 175)servoPanPosition +=stepSize; //Update the pan position variable to move the servo to the right.
    }

  }
  //Update the servo positions by sending the serial command to the Arduino.
  serial.write(tiltChannel);      //Send the tilt servo ID
  serial.write(servoTiltPosition); //Send the updated tilt position.
  serial.write(panChannel);        //Send the Pan servo ID
  serial.write(servoPanPosition);  //Send the updated pan position.
  delay(1);
}



void captureEvent(Capture c) {

  c.read();
}