Trying to update existing code.

Hey all new to programming and I require a little help. Building a kinect controlled robotic arm and am having trouble with the arduino program. I bought the Making Things See book to help me start off. problem is that the book is great for the processing part of the programming but lacks a lot in the arduino side. The code he is using is for just 2 servos and I’m trying to increase it to 4 and not having much luck. Any help you can provide will be most helpful. I will provide the original code as it is and then the changes I made.
Original:

#include <Servo.h>

//Declares the servos.
Servo shoulder;
Servo elbow;


//Setup servo positions.
int nextServo = 0;
int servoAngles[] = {0, 0};

//Define pins for each servo.
void setup()
{
  shoulder.attach(50);
  elbow.attach(51);
 
  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]);
    
  }
}

Trying to add 2 more servos:

#include <Servo.h>

//Declares the servos.
Servo shoulder;
Servo elbow;
Servo wrist;
Servo wrist2;
//Servo claw;

//Setup servo positions.
int nextServo = 0;
int servoAngles[] = {0, 0};

//Define pins for each servo.
void setup()
{
  shoulder.attach(50);
  elbow.attach(51);
  wrist.attach(52);
  wrist2.attach(53);
  //claw.attach(54);
  Serial.begin(9600);
}

void loop()
{
  if(Serial.available()>=4)
  {
    int servoAngle = Serial.read();  
    
    servoAngles[nextServo] = servoAngle;  
    nextServo++;  
   
    if(nextServo > 3)
    {
      nextServo = 0;  
    }

    shoulder.write(servoAngles[0]);
    elbow.write(servoAngles[1]);
    wrist.write(servoAngles[2]);
    wrist2.write(servoAngles[3]);
  }
}

Was there a question?

AWOL: Was there a question?

I think what my partner on this project was asking is:

We are running into issues with trying to control 4 servos on an Arduino mega. We have processing running as the bridge between the kinect and Ardunio. We can easily get 2 servos to run with no problem. But when try and add 1 or 2 more we get no response. Can anyone please help us in our code to get the other 2 servos to work? My knowledge on this code is rather limited, so any help is appreciated.

int servoAngles[] = {0, 0};

How many angles?

AWOL: int servoAngles[] = {0, 0};

How many angles?

It is a range of angles spanning about 170° from a kinect

I'll try again. How many angles?

servoAngles[nextServo] = servoAngle;  
    nextServo++;

Delete the >=4 on Serial.available().

You should consider an input system that takes which joint to move and the value. If you enter 0,20(return key) you'd read the zero as the right servo and move it to 20 degrees. To figure out how to do this (taking in the values), see http://www.tigoe.com/pcomp/code/arduinowiring/1161/ for ideas.

Also when debugging, use some Serial.println("I'm at step x"); to let you know where the code is while it is running.

TheKitty: Delete the >=4 on Serial.available().

You should consider an input system that takes which joint to move and the value. If you enter 0,20(return key) you'd read the zero as the right servo and move it to 20 degrees. To figure out how to do this (taking in the values), see http://www.tigoe.com/pcomp/code/arduinowiring/1161/ for ideas.

Also when debugging, use some Serial.println("I'm at step x"); to let you know where the code is while it is running.

Thanks I'll give that a look.

Adding serial prints may not help you when you're writing to and reading from memory you don't own.

AWOL: Adding serial prints may not help you when you're writing to and reading from memory you don't own.

Sorry for the late response. been having trouble with the arduino forums. To answer your earlier question there are 4 angles were working with and fond the first problem with the array of ServoAngles[] = {0, ,0}. That is only for 2 sets of data. That has now been changed to servoAngles[] = {0, 0, 0, 0} to account for all 4 angles were getting over the serial port. This now should at least get us on the right track. For the data that is being sent to the arduino it is this.

byte out[] = new byte[4];
out[0] = byte(shoulderAngle);
out[1] = byte(elbowAngle);
out[2] = byte(wristAngle);
out[3] = byte(wrist2Angle);
port.write(out);

OK, so it is all working?

AWOL: OK, so it is all working?

Yep, got it working really well last night. Thanks a bunch for the help. Here's a link of a video we made of the arm in action. http://www.youtube.com/watch?v=wEp0edfkdKI