PDA

View Full Version : [Question(s)] controlling 6 ax-12a's with an arduino Mega



mlerner
01-31-2013, 12:15 PM
Howdy,

I'm new to this community and I have a question for you. Sorry if this is in the wrong section also.

My goal is to control 6 servos connected to the same serial port at the same time with 6 potentiometers. I have made a program that simply controls motion of an AX-12A servo through serial port 3 of my mega with a potentiometer. I have tested the program with up to 3 servos connected in a chain to serial 3 and 3 separate potentiometers and it works fine. When I go passed 3 the program starts to fail. I am using the Tx,Rx trickery method to communicate with the servos and i'm using the RegWrite command in addition to the Action command to make the servos move. I may have configured my code wrong or I'm misunderstanding how to control the motors.

A couple side notes:
I am still using the 1000000 baud rate because I changed one of the servo's rates to a lower rate and changed the rate in my code and I could no longer communicate with that servo until i changed the rate back.

My code is not pretty:


// 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
reg_write_2_byte(2,location,old_val2);
reg_write_2_byte(3,location,old_val3);
reg_write_2_byte(4,location,old_val4);
reg_write_2_byte(5,location,old_val5);
reg_write_2_byte(6,location,old_val6);
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()
{
temp =Serial3.read();
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))%6); //Checksum Value
Serial3.write(0xFF); // Starts instruction packet
Serial3.write(0xFF);
Serial3.write(id);
Serial3.write(length);
Serial3.write(reg_write_ins);
Serial3.write(location);
Serial3.write(val&0xFF); // Lower Byte
Serial3.write((val&0xFF00) >> 8); // Upper Byte
Serial3.write(checksum);
}
/*---------------------------------------
Action Function to perform Reg_Write Functions
---------------------------------------*/
void Action(int id)
{
length = 2;
checksum = ~((id + length + action_ins)%6);
Serial3.write(0xFF);
Serial3.write(0xFF);
Serial3.write(id); // Broadcast(0xFE) is used when sending action to two Dynamixels
Serial3.write(length);
Serial3.write(action_ins);
Serial3.write(checksum);
}

mlerner
01-31-2013, 12:18 PM
another side note, the reason I used the regwrite/action commands was so I could move all the motors at the same time if need be which might be a flawed thought process since i'm new to controlling motors

Upgrayd
01-31-2013, 01:20 PM
Looks like you probably have communication collisions.

You are sending individual packets to the servos but not dealing with the individual status packets the servos return.

Three possible solutions:
Handle the returned status packets after each reg_write_2_byte().
Set all servos "Status Return Level" to zero. This would disable all status return packets.
Use the sync write packet to command all servos at the same time.

mlerner
01-31-2013, 01:58 PM
thanks for the response! I have a sync function written so i can try that as well but to start I'll go down your list of suggestions and hopefully figure some issues out.

Gertlex
01-31-2013, 02:25 PM
You might be able to diagnose something about what is going on using PyDyPackets... no guarantee though, as I've not delved into handling response packets yet.

https://github.com/erelson/PyDyPackets/
http://forums.trossenrobotics.com/showthread.php?5670-Dynamixel-packet-analyzer

mlerner
02-01-2013, 05:12 PM
I tried dealing with the packets and turning them off but the motors are still very choppy after more than 3 are connected. The sync write doesn't make the motors move in sync enough. it seems like they cascade down the line. Also I will give diagnosing the status packets a try after i turn status packets back on.
I feel like I'm missing something minute detail.

I also tried limiting the commands i send by using multiple if statements (shown below) to send only regwrite commands corresponding the pot i move but there is still a delay problem once the 4th servo is connected.
Is what i'm trying to do possible? Because I need all 6 servos to be able move at the same time without delay or choppiness.

if (present_val != old_val)
{
old_val = present_val;
reg_write_2_byte(1,location,old_val);
}

tician
02-01-2013, 05:29 PM
Are you using an adequate power supply? Adding a fourth should not affect communications unless the servos are encountering under-voltage conditions from an inadequate power supply or cables that are too long or have too many servos on a string, intermittent faults in the wire(s), or packet creation errors.

I have never gotten the reg write command to work, nor do I recall encountering anyone who ever got them to do what they claim to do. If your sync write function is correct, then all of the servos will receive a single big packet containing the broadcast ID as the packet ID with the individual servo ID and goal position within the data. None of them should start moving until the entire packet has been transmitted, but there may be some slight differences in parsing time depending on the status of the motor control loop running on the servo.