Issues upgrading Arduino code for Kinect controlled arm from 2 servos to 4

Question

I have arduino code for operating 2 servos, but we are using 4 servos and am having trouble getting the other 2 to talk.

The program so far as I can make out is that the angles for the servos that are calculated by the processing side are being sent out one after the other (shoulder, elbow, wrist, wrist2) then repeated. The arduino program gets this data and stores in into an array and then is written to the pin of the appropriate array segment. So 0 is shoulder, 1 is elbow, 2 is wrist and 3 is wirst2.

I can easily get 2 servos to run with no problem. But when I try and add 1 or 2 more we get no response. Can anyone please help me to get the other 2 servos to work? My knowledge on this code is rather limited, so any help is appreciated.

processing Data being sent to the arduino:

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

Arduino Code:

#include <Servo.h>

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

//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);

  Serial.begin(9600);
}

void loop()
{
  if(Serial.available())
  {
    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]);
  }
}

Sorry for the lengthy post but have been stuck for a while.

Answer 1

I can see two potential problems.

The first is that you're only declaring the servoAngles array to have two elements:

int servoAngles[] = {0, 0};

Your code is equivalent to this:

int servoAngles[2];
servoAngles[0] = 0;
servoAngles[1] = 0;

Later, you're assigning values to servoAngles[2] and servoAngles[3], which are out of the bounds of your original array -- this is very bad practice. Instead, you should be declaring the servoAngles array the same way you're declaring and initializing your byte out[] array -- it should be declared and/or initialized with 4 elements, as you require.

The other problem that I can see is that you're reading 1 servoAngle value every time you loop, but you're writing all 4. That means that every 4th byte is automatically going to be sent to the same servo, regardless of whether things get out of sync. (You would recognize this problem as the proper angle being put at the wrong joint.)

Normally, you'd read all 4 values at once (followed by a newline, or some special sequence of characters that indicates the end of the info) and write all 4 values at once.

Answer 2

I don't understand the purpose of the array? Why do you need to store the angle values?

Why not just to this?

elbow.write(Serial.read());

That said the over all design has a big problem. It is very easy to get out of sync if a byte is dropped. Better to write out six byte blocks with the first two bytes of each block being a unique sync word. Use some impossible value like x00ff. Then you wait for a sync word and then read the next four bytes.

The other more complex but better method is to have a checksum at the end of the block, verify the block is correct and only then write out the angles. Using both a sync word AND a checksum makes it nearly bomb proof. A written you data can have errors and you would never be able to know.