How to Attach two more Servo motors to Kinect Skeleton?

yoryeline

Hi! I am trying to solve this little problem with an example from the book "Making things see" where with the use of Kinect Processing and Arduino you can control two servo motors by moving your left arm. The kinect through the Skeleton App reads the movement of the joints on your arm. It works perfectly with one side but now I would like to add the right arm with other two servo motors. There is definitely something that I am not understanding because I have tried copying the code and replacing it with for example "SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND, SimpleOpenNI.SKEL_LEFT_SHOULDER" and so on and nothing changes. It might not be as easy as I thought. I am struggling with this that's why I decided to ask it here to see if someone could help me with a hint.

Here I post the Processing code and after the Arduino code.

Thanks in advance for your help!

import SimpleOpenNI.*;
SimpleOpenNI  kinect;
  import processing.serial.*;
     Serial port;

void setup() { 
  size(640, 480);
  kinect = new SimpleOpenNI(this);
  kinect.enableDepth();
  kinect.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);
  kinect.setMirror(true);
   println(Serial.list());
     String portName = Serial.list()[8];
     port = new Serial(this, portName, 9600);
}
void draw() {
  kinect.update();
  PImage depth = kinect.depthImage();
  image(depth, 0, 0);
  IntVector userList = new IntVector();
  kinect.getUsers(userList);
  if (userList.size() > 0) {
    int userId = userList.get(0);
    if ( kinect.isTrackingSkeleton(userId)) {
      // get the positions of the three joints of our arm
      PVector rightHand = new PVector();
      kinect.getJointPositionSkeleton(userId,
                                      SimpleOpenNI.SKEL_RIGHT_HAND,
                                      rightHand);
      PVector rightElbow = new PVector();
      kinect.getJointPositionSkeleton(userId,
                                      SimpleOpenNI.SKEL_RIGHT_ELBOW,
                                      rightElbow);
      PVector rightShoulder = new PVector();
      kinect.getJointPositionSkeleton(userId,
                                      SimpleOpenNI.SKEL_RIGHT_SHOULDER,
                                      rightShoulder);
      // we need right hip to orient the shoulder angle
      PVector rightHip = new PVector();
      kinect.getJointPositionSkeleton(userId,
                                      SimpleOpenNI.SKEL_RIGHT_HIP,
                                      rightHip);
// reduce our joint vectors to two dimensions
PVector rightHand2D = new PVector(rightHand.x, rightHand.y); 
PVector rightElbow2D = new PVector(rightElbow.x, rightElbow.y);
PVector rightShoulder2D = new PVector(rightShoulder.x,
                                            rightShoulder.y);
      PVector rightHip2D = new PVector(rightHip.x, rightHip.y);
      // calculate the axes against which we want to measure our angles
      PVector torsoOrientation =
PVector.sub(rightShoulder2D, rightHip2D); 
PVector upperArmOrientation =
        PVector.sub(rightElbow2D, rightShoulder2D);

        // calculate the angles between our joints
float shoulderAngle = angleOf(rightElbow2D, 
                                        rightShoulder2D,
                                        torsoOrientation);
          float elbowAngle    = angleOf(rightHand2D,
                                        rightElbow2D,
                                        upperArmOrientation);
          // show the angles on the screen for debugging
          fill(255,0,0);
          scale(3);
          text("shoulder: " + int(shoulderAngle) + "\n" +
                " elbow: " + int(elbowAngle), 20, 20);
     byte out[] = new byte[2];
     out[0] = byte(shoulderAngle);
     out[1] = byte(elbowAngle);
     port.write(out);
        }

} }
    float angleOf(PVector one, PVector two, PVector axis) {
      PVector limb = PVector.sub(two, one);
      return degrees(PVector.angleBetween(limb, axis));
}
    // user-tracking callbacks!
    void onNewUser(int userId) {
      println("start pose detection");
      kinect.startPoseDetection("Psi", userId);
    }
    void onEndCalibration(int userId, boolean successful) {
      if (successful) {
        println("  User calibrated !!!");
        kinect.startTrackingSkeleton(userId);
      }
      else {
        println("  Failed to calibrate user !!!");
        kinect.startPoseDetection("Psi", userId);
} }
    void onStartPose(String pose, int userId) {
      println("Started pose for user");
      kinect.stopPoseDetection(userId);
      kinect.requestCalibrationSkeleton(userId, true);
}

Arduino code

#include <Servo.h> 
// declare both servos
Servo shoulder;
Servo elbow;

// setup the array of servo positions 
int nextServo = 0;
int servoAngles[] = {0,0};

void setup() {
// attach servos to their pins 

shoulder.attach(9); 
elbow.attach(10); 
Serial.begin(9600);
}
void loop() { 
  if(Serial.available()){ 

    int servoAngle = Serial.read();
    servoAngles[nextServo] = servoAngle;
    nextServo++;
        if(nextServo > 1){
          nextServo = 0;
        }
shoulder.write(servoAngles[0]); 
elbow.write(servoAngles[1]);
} }