Move a servo motor (Ax-12) with arduino, processing and kinect

Hey guys,

I'm having a problem with my code. I wanna move a Dynamixel servo according to the movement of a person, using kinect and processing. When I tried only with the Arduino moving randomly the servo it worked perfectly, but when I try with processing+kinect it doesnt move at all. I'm gonna appreciate any help :) Oh and I'm new at this... so correct me please if y'all find dumb mistakes :P

Arduino code:

#include <DynamixelSerial.h>
int angulo=0;

void setup() {
  Dynamixel.begin(1000000, 2); // Inicialize the servo at 1Mbps and Pin Control 2
  Serial.begin(9600);
}

void loop() {

  if (Serial.available() > 0) {
  // get incoming byte:
  angulo=Serial.read();
  }

  Dynamixel.ledStatus(1, ON);
  Dynamixel.move(1,angulo);

  delay(1000);


}

Processing code:

import SimpleOpenNI.*;
import processing.serial.*;
import cc.arduino.*;
Arduino arduino;
int pos=0;
Serial myPort;

SimpleOpenNI context;
color[] userClr = new color[] { 
  color(255, 0, 0), 
  color(0, 255, 0), 
  color(0, 0, 255), 
  color(255, 255, 0), 
  color(255, 0, 255), 
  color(0, 255, 255)
};
PVector com = new PVector(); 
PVector com2d = new PVector();

void setup() {
  size(640, 480);
  println(Serial.list());
  String portName = Serial.list()[1];
  myPort = new Serial(this, portName, 9600);
  //arduino = new Arduino(this, Arduino.list()[1], 9600); //your offset may vary
  //arduino.pinMode(2);
  context = new SimpleOpenNI(this);
  if (context.isInit() == false) {
    println("Can't init SimpleOpenNI, maybe the camera is not connected!"); 
    exit();
    return;
  }
  // enable depthMap generation 
  context.enableDepth();
  // enable skeleton generation for all joints
  context.enableUser();
  context.enableRGB();
  background(200, 0, 0);
  stroke(0, 0, 255);
  strokeWeight(3);
  smooth();
}

void draw() {
  // update the cam
  context.update();
  // draw depthImageMap
  //image(context.depthImage(),0,0);
  image(context.userImage(), 0, 0);
  // draw the skeleton if it's available
  int[] userList = context.getUsers();
  for (int i=0; i<userList.length; i++) {
    if (context.isTrackingSkeleton(userList)) {
      stroke(userClr[ (userList - 1) % userClr.length ] );
      drawSkeleton(userList);
    }
  }
}



// draw the skeleton with the selected joints
void drawSkeleton(int userId) {


  // aqui é definido qual parte do corpo vai rastrear
  PVector jointPos = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, jointPos);
  PVector convertedHead = new PVector();
  context.convertRealWorldToProjective(jointPos, convertedHead);
  //desenhar uma elipse sobre a parte do corpo rastreada
  fill(255, 0, 0);
  ellipse(convertedHead.x, convertedHead.y, 20, 20);

  //draw YOUR Right Shoulder
  PVector jointPosLS = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, jointPosLS);
  PVector convertedLS = new PVector();
  context.convertRealWorldToProjective(jointPosLS, convertedLS);
  //int LS = convertedLS.x, convertedLS.y

  //draw YOUR Right Elbow
  PVector jointPosLE = new PVector();
  context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, jointPosLE);
  PVector convertedLE = new PVector();
  context.convertRealWorldToProjective(jointPosLE, convertedLE);
  fill(200, 200, 200);
  ellipse(convertedLE.x, convertedLE.y, 20, 20);


  //angulooo
  int anguloLSE =int(degrees(atan2(convertedLS.x - convertedLE.x, convertedLS.y - convertedLE.y)));
  println(anguloLSE); 
  myPort.write(anguloLSE);



  //se quiser desenhar o esqueleto inteiro, descomentar as linhas abaixo
  context.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);

  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);

  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);

  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);

  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);
}

// -----------------------------------------------------------------
// SimpleOpenNI events


void onNewUser(SimpleOpenNI curContext, int userId)
{
  println("onNewUser - userId: " + userId);
  println("\tstart tracking skeleton");

  curContext.startTrackingSkeleton(userId);
}

void onLostUser(SimpleOpenNI curContext, int userId)
{
  println("onLostUser - userId: " + userId);
}

void onVisibleUser(SimpleOpenNI curContext, int userId)
{
  //println("onVisibleUser - userId: " + userId);
}
void keyPressed()
{
  switch(key)
  {
  case ' ':
    context.setMirror(!context.mirror());
    break;
  }
}