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

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


void loop()
    int servoAngle = Serial.read();  

    servoAngles[nextServo] = servoAngle;  

    if(nextServo > 3)
      nextServo = 0;  


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

  • $\begingroup$ 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 $\endgroup$ May 13, 2013 at 14:10

2 Answers 2


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.

  • $\begingroup$ 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. $\endgroup$
    – Ian
    May 13, 2013 at 14:34
  • $\begingroup$ 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? $\endgroup$ May 13, 2013 at 18:11
  • $\begingroup$ 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. $\endgroup$
    – Ian
    May 13, 2013 at 18:31
  • $\begingroup$ 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. $\endgroup$ May 13, 2013 at 19:17
  • $\begingroup$ Got the program working smoothly. Thanks a lot for sending me in the right direction. $\endgroup$ May 14, 2013 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?


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.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.