skullduino arduino side

int servopin = 12;
int neckservo = 9;
int servo_position = 2300;
int neck_position = 2400;
int i;
int fetchPos;
int total = 0;
int average = 0;

// for serial communication
int message = 0;

void setup(){
 pinMode(servopin, OUTPUT);
  pinMode(neckservo, OUTPUT);
Serial.begin(9600);

scan(2400);
chew(2200);
delay(1000);
turnNeck(180);
turnNeck(0);
turnNeck(90);
moveJaw(21);
moveJaw(0);
//delay(5000);

}

void loop(){
  
  if (Serial.available()>0){
    //read the incoming byte
    message = Serial.read();
    
    if(message == 'z'){
      scan(700);
      scan(1100);
      scan(2500);
      scan(2600);
      scan(1100);
      scan(500);
    }
    if(message == 'e'){
      moveJaw(0);
      moveJaw(21);
      moveJaw(0);
      moveJaw(0);
      Serial.print("jaw postition = ");
      Serial.println(servo_position);
    }
    if(message == '.'){
      if(neck_position < 2400){
        turnNeck(fetchPos + 5);
      }
      else{
        turnNeck(180);
      }
    }
    if(message == ','){
      if(neck_position > 500){
        Serial.print("fetch - ");
        Serial.println(fetchPos);
        turnNeck(fetchPos - 5);
      }
      else{
        turnNeck(0);
      }
    }
    if(message == '?'){
      getHelp();
    }
    
    if(message == 'c'){
     turnNeck(90); 
    }

  }
}

void chew(int amount){
 for(i = 0;i < 10;i++){
    digitalWrite(servopin,HIGH);
    delayMicroseconds(amount);
    digitalWrite(servopin, LOW);
    delay(15); 
    servo_position = amount;
 }
}
void scan(int amount){
 for(i = 0;i < 30;i++){
    digitalWrite(neckservo,HIGH);
    delayMicroseconds(amount);
    digitalWrite(neckservo, LOW);
    delay(20); 
    neck_position = amount;
 }
}

void turnNeck(int amount){
  int currPos = map(neck_position,500,2410,0,180);
  //delay(1000);
  if(amount > currPos){
    for(int i = currPos; i <= amount;i++){
      int servoPulse = map(i,0,180,500,2410);
      digitalWrite(neckservo,HIGH);
      delayMicroseconds(servoPulse);
      digitalWrite(neckservo,LOW);
      delay(15);
  }
  
}
if(amount < currPos){
  for(int i = currPos; i > amount;i--){
      int servoPulse = map(i,0,180,500,2410);
      digitalWrite(neckservo,HIGH);
      delayMicroseconds(servoPulse);
      digitalWrite(neckservo,LOW);
      delay(15);
  }
}

neck_position = map(amount,0,180,500,2410);
fetchPos = amount;

}

void moveJaw(int amount){
  int currPos = map(servo_position,2400,1000,0,23);
  //delay(1000);
  if(amount > currPos){
    for(int i = currPos; i <= amount;i++){
      int servoPulse = map(i,0,23,2400,1000);
      digitalWrite(servopin,HIGH);
      delayMicroseconds(servoPulse);
      digitalWrite(servopin,LOW);
      delay(15);
  }
  
}
if(amount < currPos){
  for(int i = currPos; i > amount;i--){
      int servoPulse = map(i,0,23,2400,1000);
      digitalWrite(servopin,HIGH);
      delayMicroseconds(servoPulse);
      digitalWrite(servopin,LOW);
      delay(15);
  }
}

servo_position = map(amount,0,23,2400,1000);
}

void getHelp(){
  
 Serial.print("neck position is");
 Serial.println(neck_position); 
 //fetchPos = map(neck_position,500,2400,0,180);
 Serial.println(fetchPos);
 Serial.println("");
 Serial.println("jaw is at ");
 Serial.println(servo_position);
}

skullduino processing side

//import the opencv library
import hypermedia.video.*;
//initialize opencv object
OpenCV opencv;
//import serial library and minim
import processing.serial.*;
import ddf.minim.*;
//initialize values for minim
AudioPlayer song;
Minim minim;
//Declare the serial port 
Serial skullPort;
//size, baseed on output from digistix output specs
int w = 176;
int h = 144;
//threshold for difference detection
int threshold = 50;
boolean find=true;
//for font
PFont font;

int[] matchingPixel = new int[2];  //matching pixels coordinate, I use this in the easing
//floats to handle easing
float x;
float y;
float targetX, targetY;
float easing = 0.4;

void setup(){
  //window size
   size(w * 2 + 30,h);
   //openncv
   opencv = new OpenCV(this);
   opencv.capture(w,h);
   println(Serial.list()); //uncomment this line to see list of devices
   //start serial port
   skullPort = new Serial(this,Serial.list()[0],9600);
   //minim  
   minim = new Minim(this);
   //include a wav file in data folder this will play when the mouth is activated
   song = minim.loadFile("hello.wav");
  //load the font which I don't actually use so this is wasted space really, but you could have some info displayed on screen and I thought i might want to so included it
   font = loadFont( "AndaleMono.vlw" );
   textFont( font );
   }

void draw(){
  //get the image from the cam
  opencv.read();
  //flip it
  opencv.flip( OpenCV.FLIP_HORIZONTAL );
  //blur it helps with static and crap camera
  opencv.blur(OpenCV.BLUR,3);
  //display image
  image(opencv.image(),0,0);
  //flip it back
  opencv.flip( OpenCV.FLIP_HORIZONTAL );
  //get the difference
  opencv.absDiff();
  //convert to gray
  opencv.convert(OpenCV.GRAY);
  //blur alot, but you could reduce this on a better cam but I was getting lots of static
  opencv.blur(OpenCV.BLUR,15);
  //set the threshold
  opencv.threshold(threshold);
  //flip it again so it displays right
  opencv.flip( OpenCV.FLIP_HORIZONTAL );
  //display beside other
  image(opencv.image(),20+w,0);
  
  
  // find blobs
    //find blobs with a minimum of 10 pixels, max of whatever that gives you...I just played until I was happy, number of blobs = 1 can't remeber what was true..check documentation...and lastly max numbers of vertices which is straighforward
    Blob[] blobs = opencv.blobs( 10, w*h/6, 1, true, OpenCV.MAX_VERTICES*4 );
    
    fill(0,0,255);
    // draw blob results
    for( int i=0; i<blobs.length; i++ ) {
        Point centroid = blobs[i].centroid;
        beginShape();
        for( int j=0; j<blobs[i].points.length; j++ ) {
          
            vertex( blobs[i].points[j].x + 20+w, blobs[i].points[j].y );
        }
        endShape(CLOSE);
        matchingPixel[1] = centroid.x;
        matchingPixel[0] = centroid.y;
        // centroid
        stroke(255,0,255);
        strokeWeight(4);
        // caculate the goal location I do this to smooth out the blob detection location as it jumps around a little
        if((matchingPixel[1] >0) && (matchingPixel[0] > 0)){
            targetX = matchingPixel[1];
            float dx = matchingPixel[1] - x;
            if(abs(dx) > 1) {
                x += dx * easing;
             }
  
          targetY = matchingPixel[0];
          float dy = matchingPixel[0] - y;
          if(abs(dy) > 1) {
               y += dy * easing;
          }
    }
        //paste an ellipse at the easing coordinates and a crosshair at the blob centroid
        ellipse(x,y,30,30);
        line( centroid.x-5, centroid.y, centroid.x+5, centroid.y );
        line( centroid.x, centroid.y-5, centroid.x, centroid.y+5 );
        println( centroid.x );
        println( matchingPixel[1] );
        //if the ellipses center crosses to far to the right or left of the vision screen
        if(x > 125){
          skullPort.write(",");
          delay(30);
        }
        if(x < 50){
          skullPort.write(".");
          delay(30);
          //see I played around alot with when to remeber the compare image
          //opencv.remember();
        }
        //here I thought I should center when I have lost something, still not perfect it might still come in handy
       // if(centroid.x <= 101 && centroid.x >=76){
         // skullPort.write("c");
          //delay(300);
          //opencv.remember();
        //}
        noStroke();
    }

    delay(100);
    //decided on this location to remeber the background
    opencv.remember();
}

void keyPressed(){
 //manual controls for skullduino also fine tune the threshold here
 //send the key to skullduino
 skullPort.write(key);
 if(key == 'e'){
   //sent the key for mouth action now play the sound file to go with it
   song = minim.loadFile("hello.wav");
   song.play();
   
}
if(key == '='){
     if(threshold < 254){
    threshold = threshold + 5; 
   }
 }
 if(key == '-'){
     if(threshold > 3){
    threshold = threshold - 5; 
   }
 }
}
//stop stuff it's good
void stop(){
  opencv.stop();
  song.close();
  minim.stop();
  super.stop();
}