Help Please - Interfacing RS485 Controlled Servo

Forum 2005-2010 (read only) Hardware Interfacing
1

Hi Forum; I've just registered, but I've been lurking for a while.

I have searched for my problem, but I've not made any progress I'm afraid. I'm okay with basic electronics, but my programming is pretty rusty (left college many years ago!).

I've been playing with some servos I'm planning to import, and I'm fine driving 'standard' RC types (using the servo library etc.).

But one of the servo types is controlled using RS485, with a protocol apparently similar to the Dynamixel one.

It has RS485 'A' and 'B' electrical inputs for the data, using a protocol as below:

Instruction packet format:
0XFF 0XFF ID Length Instruction Parameter1 ...Parameter N Check Sum
0XFF 0XFF This signal notifies the beginning of the packet
ID It is the ID of servo which will receive Instruction Packet. It can use 254 IDs from 0 to 253 (0X00~0XFD). converts to a hexadecimal number. 0X00 - 0XFD
*Broadcasting ID : ID = 254 (0XFE) *
If Broadcast ID is used, all linked servos execute command of
Instruction Packet, and Status Packet is not returned.
LENGTH It is the length of the packet. The length is calculated as “the number of Parameters (N) + 2”.
PARAMETER0?N Parameter is used when Instruction requires ancillary data
CHECK SUM It is used to check if packet is damaged during communication. Check Sum is calculated according to the following formula:
Check Sum = ~ (ID + Length + Instruction + Parameter1 + ... Parameter N)
When the calculation result of the parenthesis in the above formula is larger than 255 (0xFF), use only lower bytes.“~” Where, “~” is the Not Bit operator,but opposite.

I would welcome any help at all about interfacing this to my Arduino Duemilanove board, specifically how to connect the RS485 A and B, and how to send it data (including how to calculate the checksum).

Thanks very much,

Ru'

ps I've tried using a Dynamixel AX-12 library I found on someone's site, but with no success. I have a DFRobot I/O Expansion shield which has an RS485 output (A & B), but their shield manual doesn't help at all with how to use it!

I'm afraid I can't post a link to the shield manual, but it can be googled using the search terms "dfrobot shield manual"

pps Baud rate is 1Mbps, asynchronous half-duplex

2

I looked too for some information on the RS485 interface on this shield, but could not find it.

The lack of documentation for this shield appears to render it useless, and you should complain. >:(

You need to know how to drive the enable output on the RS485 transciever.

What servo is this, it appears identical to robotis RX/AX, which is already done on Arbotix for instance.

3

It's a brand new Chinese servo, very similar (lol) to the RX/AX ones.

Thanks for the heads-up on Arbotix; I'll have a play with the libraries they have, although I'm a bit confused about how to connect the data lines.

I guess I may have to try and reverse engineer the PCB tracks for the I/O Expansion RS485 - I'll make sure I post a thread on here with details, as I'm not the first to ask about that particular shield.

Cheers,

Ru'

4

If you are willing to reverse engineer, check how the pins of the RS485 driver are connected especially the enable pins.

All dynamixel code uses an enable either onto the RS485 or serial TTL bus, you should just need to get the right pin. Protocol otherwise is identical.

Is the servo the SR518 ? would love to hear how it performs compared to similar servos :wink:

5

lol, well deduced Sherlock; it's an SR518 indeed :sunglasses:

6

Info on the shield is now here : http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1278937363

7

If anyone has an example sketch I can use to get this servo to do anything it would be appreciated, given that Rx is Digital Pin 0, Tx is Pin 1 and Tx enable is Pin 3 High. :cry:

8

If you want to operate the servos at 1Mbps, you ar best to borrow code from the Arbotix library sources. Standard USART routines are not fast enough to receive at 1M. If you can operate at a lower speed, then you can use standard USART routines. You will need to program the servo to the lower baud rate as well using a USB2Dynamixel or similar.
You will need to modify the setTX and setTX routines. Arbotix uses built in enable/disable on chip for direction control, you will use PD3.

Also change the USART, because Arbotix uses different USART.

9

I don't have (nor really want to buy) a USB2Dynamixel, so 1M will have to be used.

I know I'm being a) dumb, and b) needy, but any help with what I need to modify will be appreciated.

10

Something like this should work. I don't have servo or interface to test.

/*
Test Sketch for RS485 Servo on Arduino IO shield
** Uses hardware serial port, so this cannot be used at same time by FTDI USB**
** Serial speed is 1Mbps **
Based on: ax12.cpp - arbotiX Library for AX-12 Servos by Michael E. Ferguson
*/

#include <avr/interrupt.h>


#define AX12_BUFFER_SIZE            32

/** EEPROM AREA **/
#define AX_MODEL_NUMBER_L           0
#define AX_MODEL_NUMBER_H           1
#define AX_VERSION                  2
#define AX_ID                       3
#define AX_BAUD_RATE                4
#define AX_RETURN_DELAY_TIME        5
#define AX_CW_ANGLE_LIMIT_L         6
#define AX_CW_ANGLE_LIMIT_H         7
#define AX_CCW_ANGLE_LIMIT_L        8
#define AX_CCW_ANGLE_LIMIT_H        9
#define AX_SYSTEM_DATA2             10
#define AX_LIMIT_TEMPERATURE        11
#define AX_DOWN_LIMIT_VOLTAGE       12
#define AX_UP_LIMIT_VOLTAGE         13
#define AX_MAX_TORQUE_L             14
#define AX_MAX_TORQUE_H             15
#define AX_RETURN_LEVEL             16
#define AX_ALARM_LED                17
#define AX_ALARM_SHUTDOWN           18
#define AX_OPERATING_MODE           19
#define AX_DOWN_CALIBRATION_L       20
#define AX_DOWN_CALIBRATION_H       21
#define AX_UP_CALIBRATION_L         22
#define AX_UP_CALIBRATION_H         23
/** RAM AREA **/
#define AX_TORQUE_ENABLE            24
#define AX_LED                      25
#define AX_CW_COMPLIANCE_MARGIN     26
#define AX_CCW_COMPLIANCE_MARGIN    27
#define AX_CW_COMPLIANCE_SLOPE      28
#define AX_CCW_COMPLIANCE_SLOPE     29
#define AX_GOAL_POSITION_L          30
#define AX_GOAL_POSITION_H          31
#define AX_GOAL_SPEED_L             32
#define AX_GOAL_SPEED_H             33
#define AX_TORQUE_LIMIT_L           34
#define AX_TORQUE_LIMIT_H           35
#define AX_PRESENT_POSITION_L       36
#define AX_PRESENT_POSITION_H       37
#define AX_PRESENT_SPEED_L          38
#define AX_PRESENT_SPEED_H          39
#define AX_PRESENT_LOAD_L           40
#define AX_PRESENT_LOAD_H           41
#define AX_PRESENT_VOLTAGE          42
#define AX_PRESENT_TEMPERATURE      43
#define AX_REGISTERED_INSTRUCTION   44
#define AX_PAUSE_TIME               45
#define AX_MOVING                   46
#define AX_LOCK                     47
#define AX_PUNCH_L                  48
#define AX_PUNCH_H                  49
/** Status Return Levels **/
#define AX_RETURN_NONE              0
#define AX_RETURN_READ              1
#define AX_RETURN_ALL               2
/** Instruction Set **/
#define AX_PING                     1
#define AX_READ_DATA                2
#define AX_WRITE_DATA               3
#define AX_REG_WRITE                4
#define AX_ACTION                   5
#define AX_RESET                    6
#define AX_SYNC_WRITE               131



/* Pins used to enable and disable RS485 or TTL drivers
only EN_PIN is used for RS485 (RX series Dynamixel)
Both used to enable/disable HC125 for TTL (AX series Dynamixel)
*/

#define EN_TX_PIN  3  // pin used to disable driver high during receive
#define EN_RX_PIN 2  // pin used to enable receiver low during receive

unsigned char ax_rx_buffer[AX12_BUFFER_SIZE];
unsigned char ax_tx_buffer[AX12_BUFFER_SIZE];
unsigned char ax_rx_int_buffer[AX12_BUFFER_SIZE];



// making these volatile keeps the compiler from optimizing loops of available()
volatile int ax_rx_Pointer;
volatile int ax_tx_Pointer;
volatile int ax_rx_int_Pointer;



void setup(){
    ax12Init(1000000);
    pinMode(EN_TX_PIN, OUTPUT);
    pinMode(EN_RX_PIN, OUTPUT);
}



void loop(){
  ax12SetRegister2( 1,  AX_GOAL_POSITION_L , 100);
  delay(1000);
  ax12SetRegister2( 1,  AX_GOAL_POSITION_L , 900);
  delay(1000);
  
  
}

/** initializes serial0 transmit at baud, 8-N-1 * */
void ax12Init(long baud){
    UBRR0H = ((F_CPU / 16 + baud / 2) / baud - 1) >> 8;
    UBRR0L = ((F_CPU / 16 + baud / 2) / baud - 1);
    UCSR0C = (3 << UCSZ00);
    bitClear(UCSR0B, RXCIE0);
    bitSet(UCSR0B, RXEN0);    
    bitSet(UCSR0B, TXEN0);

    
    ax_rx_int_Pointer = 0;
    ax_rx_Pointer = 0;
    ax_tx_Pointer = 0;
    // enable rx
    setTX();    
}

/** Sends a character out the serial port. */
void ax12write(unsigned char data){
    while (bit_is_clear(UCSR0A, UDRE0));
    UDR0 = data;
}
/** Sends a character out the serial port, and puts it in the tx_buffer */
void ax12writeB(unsigned char data){
    ax_tx_buffer[(ax_tx_Pointer++)] = data; 
    while (bit_is_clear(UCSR0A, UDRE0));
    UDR0 = data;
}
/** We have a one-way recieve buffer, which is reset after each packet is receieved. 
    A wrap-around buffer does not appear to be fast enough to catch all bytes at 1Mbps. */
ISR(USART_RX_vect){
    ax_rx_int_buffer[(ax_rx_int_Pointer++)] = UDR0;
}

/** read back the error code for our latest packet read */
int ax12Error;
/** > 0 = success */
int ax12ReadPacket(int length){
    unsigned long ulCounter;
    unsigned char offset, blength, checksum, timeout;
    unsigned char volatile bcount; 

    offset = 0;
    timeout = 0;
    bcount = 0;
    while(bcount < length){
        ulCounter = 0;
        while((bcount + offset) == ax_rx_int_Pointer){
            if(ulCounter++ > 1000L){ // was 3000
                timeout = 1;
                break;
            }
        }
        if(timeout) break;
        ax_rx_buffer[bcount] = ax_rx_int_buffer[bcount + offset];
        if((bcount == 0) && (ax_rx_buffer[0] != 0xff))
            offset++;
        else
            bcount++;
    }

    blength = bcount;
    checksum = 0;
    for(offset=2;offset<bcount;offset++)
        checksum += ax_rx_buffer[offset];
    if((checksum%256) != 255){
        return 0;
    }else{
        return 1;
    }
}

void waitTXC(void){
      bitSet(UCSR0A, TXC0);
      while (bit_is_clear(UCSR0A, TXC0));

}

void setTX(void){
    bitClear(UCSR0B, RXCIE0);

    bitSet(PORTD, EN_TX_PIN);
    bitClear(PORTD, EN_RX_PIN); 
}

void setRX(void){
    bitSet(UCSR0B, RXCIE0);
 
    bitSet(PORTD, EN_RX_PIN);
    bitClear(PORTD, EN_TX_PIN); 
}

/******************************************************************************
 * Packet Level
 */

/** Read register value(s) */
int ax12GetRegister(int id, int regstart, int length){  
    setTX();
    // 0xFF 0xFF ID LENGTH INSTRUCTION PARAM... CHECKSUM    
    int checksum = ~((id + 6 + regstart + length)%256);
    ax12writeB(0xFF);
    ax12writeB(0xFF);
    ax12writeB(id);
    ax12writeB(4);    // length
    ax12writeB(AX_READ_DATA);
    ax12writeB(regstart);
    ax12writeB(length);
    ax12writeB(checksum); 
    waitTXC(); 
    setRX();    
    if(ax12ReadPacket(length + 6) > 0){
        ax12Error = ax_rx_buffer[4];
        if(length == 1)
            return ax_rx_buffer[5];
        else
            return ax_rx_buffer[5] + (ax_rx_buffer[6]<<8);
    }else{
        return -1;
    }
}

/* Set the value of a single-byte register. */
void ax12SetRegister(int id, int regstart, int data){
    setTX();    
    ax12writeB(0xFF);
    ax12writeB(0xFF);
    ax12writeB(id);
    ax12writeB(4);    // length
    ax12writeB(AX_WRITE_DATA);
    ax12writeB(regstart);
    ax12writeB(data&0xff);
    // checksum = 
    ax12writeB(0xFF - ((id + 4 + AX_WRITE_DATA + regstart + (data&0xff)) % 256) );
    waitTXC();
    setRX();
    //ax12ReadPacket();
}
/* Set the value of a double-byte register. */
void ax12SetRegister2(int id, int regstart, int data){
    setTX();    
    ax12writeB(0xFF);
    ax12writeB(0xFF);
    ax12writeB(id);
    ax12writeB(5);    // length
    ax12writeB(AX_WRITE_DATA);
    ax12writeB(regstart);
    ax12writeB(data&0xff);
    ax12writeB((data&0xff00)>>8);
    // checksum = 
    ax12writeB(0xFF - ((id + 5 + AX_WRITE_DATA + regstart + (data&0xFF) + ((data&0xFF00)>>8)) % 256) );
    waitTXC();
    setRX();
    //ax12ReadPacket();
}

Just move servo back and forth.

You will need to ensure RS485 driver does not interfere with download.

11

i-Bot, you're a *ing star!

Typically I can't test this out straight away 'cos working nights, and I'm just about to leave :frowning:

However, I'll give this a go as soon as I can.

Just one question about this bit:

#define EN_TX_PIN 3 // pin used to disable driver high during receive
#define EN_RX_PIN 2 // pin used to enable receiver low during receive

I think pin 2 needs to be low to receive (and high to transmit) - does this fit with the above? Data received appears on pin 0 (data transmitted goes to pin 1).

Thanks again,

Ru'

Edit - now I've confused myself with my tracing of the RS485 on the IO Expansion shield - I'm not sure about my question above now. No time, must go. Aaargh.

12

Yep; I got my info on the I/O Expansion Shield wrong (damn software engineers wih their 'pin 0', lol...) - when I said pin 3 I actually meant pin 2.

Pin 2 needs to be taken high to transmit, and low to receive (ignore pin 3!). ::slight_smile:

13

Sorry, there was an error in the SetRX routine which prevented reading registers. :-[ Fix is.

void setRX(void){
    bitSet(UCSR0B, RXCIE0);
 
    bitClear(PORTD, EN_RX_PIN);
    bitSet(PORTD, EN_TX_PIN); 
    ax_rx_int_Pointer = 0;
    ax_rx_Pointer = 0;
}

Also the ax_tx_buffer part could be removed.

When it works for you on the shield, we can put up a single correct clean version.

14

Sadly I'm getting an error:

avrdude: stk500_getsync(): not in sync: resp=0xff
avrdude: stk500_disable(): protocol error, expect=0x14, resp=0xff

I'm so thick I'm not even sure what this is saying I'm afraid - any pointers would be gratefully received! :-[

15

I'm not even sure what this is saying

It is saying that your arduino board doesn't work.

16

The RS485 interface uses the same pins as the connection from the Arduino processor to to the USB interface. The error suggest the RS485 driver is compromising that connection during download. The RS485 driver will override the USB RX due to the resistors between the USB and the processor.

During download you will need to disconnect the RS485 receiver from the CPU RX pin using the links on the IO expansion shield. Reconnect after download if you want to receive from the servo.

Don't Panic :sunglasses:

17

Thanks i-Bot (remember you need to type any replies to me very slowly ;D )

So:

  1. Isolate the IO Shield RS485 using the pcb jumpers.
  2. Upload the sketch to the Arduino.
  3. Re-connect the RS485 jumpers.
  4. See servo run (run, servo, run).

How do I see any data response from the servo?

18

Did you get to the "(run, servo, run)2 yet ?

Seeing the data response is tough. Because the servo runs at 1M it must use the hardware UART. This is why robot folks tend to use Arbotix, because it has two UARTS, so you can talk to PC and servo at same time.

19

Been working, so no servo run'age yet. Will post when I get a chance for more testing...

20

Doesn't move, sadly.

Couple of questions if you're still willing to help:

  1. Could you explain this bit:

#define EN_TX_PIN 3 // pin used to disable driver high during receive
#define EN_RX_PIN 2 // pin used to enable receiver low during receive

  1. When you say "Also the ax_tx_buffer part could be removed." do you mean this bit:

unsigned char ax_rx_buffer[AX12_BUFFER_SIZE];
unsigned char ax_tx_buffer[AX12_BUFFER_SIZE];
unsigned char ax_rx_int_buffer[AX12_BUFFER_SIZE];

Thanks again,
Ru'