WebbotLib AVR library
WebbotLib It just does it
  C++ documentation  C documentation

Dynamixel/AX12.h

Adds support for the Dynamixel AX-12 servo.
See http://www.trossenrobotics.com/dynamixel-ax-12-robot-actuator.aspx for more info and a data sheet.
DynamixelAX12.jpg
These servos are controlled over a UART in half-duplex mode and allows multiple servos to be controlled via the same UART. You MUST make sure that each servo has been changed to have a unique ID number as described in the manual.
 
HalfDuplex.jpg
The half-duplex mode means that a single wire is used to transmit data to the servo and is also used to listen for any response. In order to connect this to the UART on your board you will need some additional circuitry like this:
Note that you only need to make one of these circuits regardless of the number of servos you are controlling. The effect of the circuit is to use the 'Direction' I/O line to connect the 'Data' line to the 'Tx' pin of your UART, or to the the 'Rx' pin of your UART - but never to both.
The WebbotLib driver takes care of this for you by setting the direction pin correctly at all times.

 

Function

 


MAKE_DYNAMIXEL_AX12(boolean inverted, uint8_t, id, boolean continuous)

A macro to create a new AX12 servo.
The 'inverted' parameter has the same meaning as with other servos and motors. See actuators.h
The 'id' parameter is the unique ID number for the servo.
The 'continuous' parameter should be set to TRUE if you want to use the servo in continuous rotation mode (ie as a motor). FALSE is used to specify that it functions as a servo.
The servos must then be grouped into a DYNAMIXEL_AX12_LIST and finally we create a driver using MAKE_DYNAMIXEL_AX12_DRIVER specifying the list, the hardware UART we want to use, and which I/O pin to use for the direction line. Note that due to the high communication speed required you must use a hardware UART.
Example:-
// Create 2 servos
DYNAMIXEL_AX12 servo1 = MAKE_DYNAMIXEL_AX12( FALSE, 2, FALSE );
DYNAMIXEL_AX12 servo2 = MAKE_DYNAMIXEL_AX12( FALSE, 3, FALSE );
// Put them in a list
DYNAMIXEL_AX12_LIST PROGMEM servos = { &servo1, &servo2 };
// Create a driver using UART1
DYNAMIXEL_AX12_DRIVER driver = MAKE_DYNAMIXEL_AX12_DRIVER( &servos, UART0, F0);
Once set up the servo position, or speed, can be controlled like any other servo by using:

void ax12Init(DYNAMIXEL_AX12_DRIVER* driver, BAUD_RATE baud)

Initialise the driver to a given baud rate.
This must be called before the servos can be used. It is usually called from your appInitHardware function.
The default baud rate is 1000000. Yes: 1 million bits per second !

uint16_t ax12GetInfo(DYNAMIXEL_AX12* servo)

Read the current settings from a servo.
The return value is 0 if the command executed correctly and the data in the servos 'info' structure has been updated. A non-zero value indicates an error and is made up of a number of error bits ORed together. Look in the AX12.h file to see the list of error codes.
If the call is successful then the info structure is updated with the current position, speed, load, voltage, temperature and whether or not the servo is moving.

void ax12Dump(DYNAMIXEL_AX12* servo)

Dump the current status of a given servo to the rprintf destination.
This will execute an ax12GetInfo and either print out the error message or print out the information returned.
Also see: ax12DumpAll

void ax12DumpAll(const DYNAMIXEL_AX12_DRIVER* driver)

Dump the status of ALL of the servos to the rprintf destination.
This allows you to view a snapshot of all of the servos connected to one AX12 driver.

void ax12Begin(DYNAMIXEL_AX12_DRIVER* driver)

Marks the start of a group of commands.
These servos are often used in humanoid robots and given the number of servos involved and time taken to update them all then you may notice each servo moving one after the other.
This command allows you to mark the start of a group of commands and would normally be placed at the start of your appControl loop. The commands are sent out to the servos in the main loop but they are only acted upon when an ax12End command is received at the end of your main loop.

void ax12End(DYNAMIXEL_AX12_DRIVER* driver)

Marks the end of a command group.
See ax12Begin for a description of the Begin/End commands.

void ax12SetXXXX(servo,val)

Macros to allow advanced users to set a value in the servos Control Table.
Look inside Dynamixel/AX12.h to see the different options for XXXX.
Each of the macros expects the address of a servo and the 8 or 16 bit value to be written.
For example: to turn on the LED for servo12 then use:
ax12SetLED(&servo12, 1);

void ax12Send(const DYNAMIXEL_AX12_DRIVER* driver, uint8_t id, size_t len, uint8_t* data)

Send a sequence of bytes to the AX12.
The byte sequence should start with 0xff, 0xff and end in the calculated checksum.
To make life easier the AX12.h file defines a set of macros for setting individual values on a servo. These macros all start with 'ax12_set_'.
For example to set the position of servo1 to a value of 100 then:
ax12_set_GOAL_POSITION(&servo1, 100);
However: since the AX12 is an actuator then you can also use the functions defined in actuators.h
So to set the servo to its center position we could use:
act_setSpeed(servo1, 0);

Valid XHTML 1.0 Transitional