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

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())
{

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.

• Change int servoAngles[] = {0, 0}; to int servoAngles[4]; or to int servoAngles[] = {0,0,0,0}; Also, run a test with the original code but 51 changed to 52 or 53 to check hardware – James Waldby - jwpat7 May 13 '13 at 14:10

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.

• I would also recommend testing your code by ignoring the serial port, and sending a series of pre-scripted angles to your arm. In other words, separate the code that reads the serial port from the code that sets the angles. – Ian May 13 '13 at 14:34
• Ok thank you for your input. That does help a lot. So for reading all 4 values at once would the best way be to put them all into the serial buffer, write them to the array and then flush the buffer to start over again? – Fraxagoracul May 13 '13 at 18:11
• It depends on the format of the data that you're expecting, but rather than read a single byte at a time you could use use ReadBytes or ReadBytesUntil to read multiple bytes. Then when that function returns, write all 4 values to their respective servos. – Ian May 13 '13 at 18:31
• OK so made some changes. Haven't had to test it out yet. But can you tell me if i'm going in the right direction? byte servoAngle[4]; Serial.readBytes((char*)servoAngles, 4); Then write to the servos. – Fraxagoracul May 13 '13 at 19:17
• Got the program working smoothly. Thanks a lot for sending me in the right direction. – Fraxagoracul May 14 '13 at 3:56

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

Why not just to this?