Mega and 6 Ax-12A Servos

So I have written a program to control 6 AX-12A servos chained together with 6 potentiometers. The servos are powered by a lab power supply at 10 volts. I have connected the data wire of the servos to serial port 3 on the mega (pins 14, 15) and the program switches between transmitting and receiving to interface with the AX12A’s half duplex UART. The program works just fine for 3 servos but as I start to go past 3 everything becomes choppy and delayed. I’m not quite sure what the problem is. Any idea for a possible solution? The code is listed below:

/* This Code is used with an Arduino Mega to communicate with Dynamixel
with Dynamixel AX-12A Servo Motor. All commands use Serial 3 to write 
to servo and Serial to write to pc for purpose of recieving status packets

// instruction table for Dynamixel AX-12A
#define ping_ins 1 // checked
#define read_ins 2 // checked
#define write_ins 3 // Checked
#define reg_write_ins 4 // Checked 
#define action_ins 5 // Checked
#define reset_ins 6 // Checked 
#define sync_write_ins 131 


//registers table for Dynamixel AX-12A
#define servo_id 3
#define baud_rate 4
#define max_torque 14
#define alarm_led 17
#define alarm_shutdown 18
#define torque_enable 24
#define goal_position 30
#define moving_speed 32

// Code Begins...

int length,location,temp,checksum,N,L; // used for status packet & return packet
int old_val = 0;
int old_val2 = 0;
int old_val3 = 0;
int old_val4 = 0;
int old_val5 = 0;
int old_val6 = 0;

int present_val = 0;
int present_val2 = 0;
int present_val3 = 0;
int present_val4 = 0;
int present_val5 = 0;
int present_val6 = 0;

int id;
int analogPin = A0;  // 5 Potenimeters. each 1 controls 1 motor
int analogPin2 = A1;
int analogPin3 = A2;
int analogPin4 = A3;
int analogPin5 = A4;
int analogPin6 = A5;
int value;

void setup()
  Serial3.begin(1000000); // Servo Communication Speed
  Serial.begin(9600); // Communication speed Arduino/PC

void loop()

  location = goal_position;
  present_val = analogRead(analogPin); // read value of A0
  present_val2 = analogRead(analogPin2);
  present_val3 = analogRead(analogPin3);
  present_val4 = analogRead(analogPin4);
  present_val5 = analogRead(analogPin5);
  present_val6 = analogRead(analogPin6);
  if ((present_val != old_val)||(present_val2 != old_val2)||
  (present_val3 != old_val3)||(present_val4 != old_val4)||
  (present_val5 != old_val5)||(present_val6 != old_val6)) // 
    old_val = present_val; // old_val to present_val
    old_val2 = present_val2;
    old_val3 = present_val3;
    old_val4 = present_val4; 
    old_val5 = present_val5;
    old_val6 = present_val6;
    transmit(); //enable trasmission
   // Reg_Write Instructions
   reg_write_2_byte(1,location,old_val);// put location value into buffer
   Action(0xFE); //execute buffer
    recieve(); //enable recieving


void transmit()
  bitSet(UCSR3B,3); // Sets Tx pin
  bitClear(UCSR3B,4); // Clear Rx pin
  bitClear(UCSR3B,7); // Disable Rx Interrupt

void recieve()
  bitClear(UCSR3B,3); // Clear Tx pin
  bitSet(UCSR3B,4);  // Set Rx Pin
  bitSet(UCSR3B,7);  // Allows Rx Interrupt 

//Recieve Interrupt Subroutine

void serialEvent3()
  Serial.print(temp,HEX); // prints incoming return packet bit 

 Function to reg_write to a 2 byte register
 location = what register to write to
 val = what value to write to register
void reg_write_2_byte(int id, int location,int val)
  length = 5; // length of 2-byte instruction is 5
  checksum = ~((id + length + reg_write_ins + location + (val&0xFF) + ((val&0xFF00) >> 8))%256); //Checksum Value
  Serial3.write(0xFF); // Starts instruction packet
  Serial3.write(val&0xFF); // Lower Byte 
  Serial3.write((val&0xFF00) >> 8); // Upper Byte 
 Action Function to perform Reg_Write Functions
void Action(int id)
  length = 2;
  checksum = ~((id + length + action_ins)%256);
  Serial3.write(id); // Broadcast(0xFE) is used when sending action to two Dynamixels 
int analogPin = A0;  // 5 Potenimeters. each 1 controls 1 motor
int analogPin2 = A1;
int analogPin3 = A2;
int analogPin4 = A3;
int analogPin5 = A4;
int analogPin6 = A5;

Nothing, 2, 3, 4, 5, 6, huh?

Have you heard of arrays? You code would be much shorter if you used arrays.

  Serial3.begin(1000000); // Servo Communication Speed

That's 8 times the highest supported speed.

Is it necessary to send 6 values because one changed?

The high baud rate is the servo's default. I tried lowering the rate for the servos using dynamixel wizard (a program that allows connection between servo and computer) and in the code but then i couldn't communicate with the servos via mega any more. It was odd. The reason I send 6 messages is because multiple potentiometers turn at once to control the motors.

is it just not possible or am I missing something? I'm assuming I'm missing something but so far I haven't been able to find any solutions online.

These servos use serial communications. Why are you not using the conventional Serial interface functions ?

i'm sorry, i'm not sure I know the commands that you're speaking of. Some of the code is written so that I can use connect both the Tx and Rx pins to the 1 data line of the servos and communicate. I know there are libraries for these servos but they don't use this method

It's an async half duplex protocol. To do this properly the Arduino UART would need to work in half duplex mode. I don't know whether it supports this mode or how it would be achieved.

The approach you're using of shorting the Tx and Rx pins together is a bodge which would result in the Arduino receiving an 'echo' of every command it sent to the servos. I suppose it could be made to work as long as you deal with the echo correctly. However, you don't deal with it. You ignore it. You also ignore the servos' responses to your commands, which means your following command transmission tramples all over the previous servo's response to the previous command.

For this software-based async approach to work you need to keep track of the command/response nature of the protocol and wait until the response has been received before sending the next command. IMO, configuring the UART to work in half-duplex mode, if possible, would work much better.

How would i deal with the echo? I turned off the status packets as to avoid transmission errors and I have just implemented the buffer schematic as shown in the AX-12A data sheet. I changed my code to account for the buffer but the problem persists. More than 3 servos connected together and the the performance drops off. I can still move 4 servos but there is an unacceptable delay between turning a pot and the motion of the corresponding servo.

If you’ve configured the servos so they don’t send any responses, you’re much better off. Now you just need to deal with congestion and latency in the serial pipeline.

I suggest that for a start you only send commands to those servos you actually need to move, and that you control the frequency at which you send those commands so that you don’t flood the serial link. The goal here is to keep the peak serial data rate below the rate that the serial stream can actually carry, so that you don’t get congestion at the sender or receiver. The moment you start over-running that stream you’ll get back into latency problems so make sure you’re fairly conservative about the transmission rate.

How would you suggest controlling the frequency?
I tried changing the if statement to multiple ifs (shown below) to only send 1 regwrite command when a given pot is moved. Once that 4th servo is connected, there is still a delay.
Is what I’m trying to do possible? Because the servos need to be able to move all at the same time without delay or choppiness.

 if ((present_val > old_val+2)||(present_val < old_val-2))
    old_val = present_val;

mlerner: How would you suggest controlling the frequency?

After you send a command, don't send another command until enough time has elapsed to be certain that the command has finished transmission. It is pointless (and actually counterproductive) to poll the state of your inputs looking for changes that need to be commanded, until you are in a position to actually send the next command.