Robotis Dynamixel AX-12A
The first order of business is making the servos move.
First, I found the handy Dynamixel AX-12A datasheet. A slimmed down version is here. These describe the serial interface, the instruction set, and how to create an instruction packet.
Some key points are that the servos run daisy chained on a half duplex serial bus. This means that on your UART hardware, you need to connect the RX and TX lines. This is easily done on the Arbotix Robocontroller with a jumper. It also says that the max communication speed is 1Mbps and provides details on the format of the instruction packet.
Honestly, this part isn’t especially thrilling to me, so i borrowed the ax12 library from the Arbotix bioloid library. This library includes handy low level functions like the basic data write to the bus. I downloaded the full Arbotix libraries here. I copied the ax12.cpp and ax12.h files in the arbotix\libraries\Bioloid directory into my Arduino sketch. This way i could modify them without affecting the public library.
I got pretty far using just these two files. I first initialized the servo serial interface by calling ‘ax12Init(1000000);’ in my Setup() loop. This initializes the serial port and sets it to the maximum 1Mbps baud rate. Full throttle or bust I say.
To command a servo to a position, I simply call ‘SetPosition(id, pos);’ from the library. ID is the ID of my servo and POS is the commanded position. Here’s how the AX-12 maps position:
AX-12A Position Orientation
Note that it’s a little strange how the Position is oriented on the servo. Position ranges from 0 to 1024 which corresponds to angle 0 to 300. There is a 60 degree dead space at the bottom of the servo and Positon 0 starts 30degrees off from where you’d expect to start. I designed my hex so that 0 degrees was pointing straight up and the servo could angle +/- CW or CCW from there. The code to convert my angle system to Dynamix positioin looks like:
servoPosition = round((abs( anglePosition - 210) - 60 ) / 0.293);
Where ‘anglePosition’ is the angle I want using my orientation/nomenclature and ‘servoPositoin’ is the corresponding position in AX-12 speak.
But wait, there’s more!
So using the using the above SetPosition function from the ax12 library, I’m sending one command instruction per servo. This is slow and just generally ugly when there is slick ‘SYNC WRITE’ function in the AX-12 instruction set. The only problem is that there isn’t such a function in the ax12 library to use. But hey, I’m supposed to be doing this all myself right?
Page 23 of the datasheet explains the SYNC WRITE instruction and how to set it up. Basically with one instruction, you can command a whole range of servos, each to their own Goal Position. Nice and fast and neat. Here’s what my rather ugly function looks like:
/****************************************************************
ax12SyncWriteServos()
Writes positions to all servos at once using sync write
*****************************************************************/
void ax12SyncWriteServos(){
int length = (2 + 1)* NUM_SERVOS + 4; // (L + 1) * N + 4 (L: Data length for each Dynamixel actuator, N: The number of Dynamixel actuators)
int checksum = 254 + length + AX_SYNC_WRITE + 2 + AX_GOAL_POSITION_L;
setTXall();
ax12write(0xFF); // Start bits
ax12write(0xFF); // Start bits
ax12write(0xFE); // Broadcast ID for all servos
ax12write(length);
ax12write(AX_SYNC_WRITE); // Instruction
ax12write(AX_GOAL_POSITION_L); // Parameter 1: Starting address of the location where the data is to be written
ax12write(2); // Parameter 2: The length of the data to be written (L)
//RIGHT FRONT
checksum += RF_COXA_ID + (leg[RIGHT_FRONT].servoPos.coxa & 0xff) + (leg[RIGHT_FRONT].servoPos.coxa >> 8);
ax12write(RF_COXA_ID);
ax12write(leg[RIGHT_FRONT].servoPos.coxa & 0xff);
ax12write(leg[RIGHT_FRONT].servoPos.coxa >> 8);
checksum += RF_FEMUR_ID + (leg[RIGHT_FRONT].servoPos.femur & 0xff) + (leg[RIGHT_FRONT].servoPos.femur >> 8);
ax12write(RF_FEMUR_ID);
ax12write(leg[RIGHT_FRONT].servoPos.femur & 0xff);
ax12write(leg[RIGHT_FRONT].servoPos.femur >> 8);
checksum += RF_TIBIA_ID + (leg[RIGHT_FRONT].servoPos.tibia & 0xff) + (leg[RIGHT_FRONT].servoPos.tibia >> 8);
ax12write(RF_TIBIA_ID);
ax12write(leg[RIGHT_FRONT].servoPos.tibia & 0xff);
ax12write(leg[RIGHT_FRONT].servoPos.tibia >> 8);
//RIGHT MIDDLE
checksum += RM_COXA_ID + (leg[RIGHT_MIDDLE].servoPos.coxa & 0xff) + (leg[RIGHT_MIDDLE].servoPos.coxa >> 8);
ax12write(RM_COXA_ID);
ax12write(leg[RIGHT_MIDDLE].servoPos.coxa & 0xff);
ax12write(leg[RIGHT_MIDDLE].servoPos.coxa >> 8);
checksum += RM_FEMUR_ID + (leg[RIGHT_MIDDLE].servoPos.femur & 0xff) + (leg[RIGHT_MIDDLE].servoPos.femur >> 8);
ax12write(RM_FEMUR_ID);
ax12write(leg[RIGHT_MIDDLE].servoPos.femur & 0xff);
ax12write(leg[RIGHT_MIDDLE].servoPos.femur >> 8);
checksum += RM_TIBIA_ID + (leg[RIGHT_MIDDLE].servoPos.tibia & 0xff) + (leg[RIGHT_MIDDLE].servoPos.tibia >> 8);
ax12write(RM_TIBIA_ID);
ax12write(leg[RIGHT_MIDDLE].servoPos.tibia & 0xff);
ax12write(leg[RIGHT_MIDDLE].servoPos.tibia >> 8);
//RIGHT REAR
checksum += RR_COXA_ID + (leg[RIGHT_REAR].servoPos.coxa & 0xff) + (leg[RIGHT_REAR].servoPos.coxa >> 8);
ax12write(RR_COXA_ID);
ax12write(leg[RIGHT_REAR].servoPos.coxa & 0xff);
ax12write(leg[RIGHT_REAR].servoPos.coxa >> 8);
checksum += RR_FEMUR_ID + (leg[RIGHT_REAR].servoPos.femur & 0xff) + (leg[RIGHT_REAR].servoPos.femur >> 8);
ax12write(RR_FEMUR_ID);
ax12write(leg[RIGHT_REAR].servoPos.femur & 0xff);
ax12write(leg[RIGHT_REAR].servoPos.femur >> 8);
checksum += RR_TIBIA_ID + (leg[RIGHT_REAR].servoPos.tibia & 0xff) + (leg[RIGHT_REAR].servoPos.tibia >> 8);
ax12write(RR_TIBIA_ID);
ax12write(leg[RIGHT_REAR].servoPos.tibia & 0xff);
ax12write(leg[RIGHT_REAR].servoPos.tibia >> 8);
//LEFT REAR
checksum += LR_COXA_ID + (leg[LEFT_REAR].servoPos.coxa & 0xff) + (leg[LEFT_REAR].servoPos.coxa >> 8);
ax12write(LR_COXA_ID);
ax12write(leg[LEFT_REAR].servoPos.coxa & 0xff);
ax12write(leg[LEFT_REAR].servoPos.coxa >> 8);
checksum += LR_FEMUR_ID + (leg[LEFT_REAR].servoPos.femur & 0xff) + (leg[LEFT_REAR].servoPos.femur >> 8);
ax12write(LR_FEMUR_ID);
ax12write(leg[LEFT_REAR].servoPos.femur & 0xff);
ax12write(leg[LEFT_REAR].servoPos.femur >> 8);
checksum += LR_TIBIA_ID + (leg[LEFT_REAR].servoPos.tibia & 0xff) + (leg[LEFT_REAR].servoPos.tibia >> 8);
ax12write(LR_TIBIA_ID);
ax12write(leg[LEFT_REAR].servoPos.tibia & 0xff);
ax12write(leg[LEFT_REAR].servoPos.tibia >> 8);
//LEFT MIDDLE
checksum += LM_COXA_ID + (leg[LEFT_MIDDLE].servoPos.coxa & 0xff) + (leg[LEFT_MIDDLE].servoPos.coxa >> 8);
ax12write(LM_COXA_ID);
ax12write(leg[LEFT_MIDDLE].servoPos.coxa & 0xff);
ax12write(leg[LEFT_MIDDLE].servoPos.coxa >> 8);
checksum += LM_FEMUR_ID + (leg[LEFT_MIDDLE].servoPos.femur & 0xff) + (leg[LEFT_MIDDLE].servoPos.femur >> 8);
ax12write(LM_FEMUR_ID);
ax12write(leg[LEFT_MIDDLE].servoPos.femur & 0xff);
ax12write(leg[LEFT_MIDDLE].servoPos.femur >> 8);
checksum += LM_TIBIA_ID + (leg[LEFT_MIDDLE].servoPos.tibia & 0xff) + (leg[LEFT_MIDDLE].servoPos.tibia >> 8);
ax12write(LM_TIBIA_ID);
ax12write(leg[LEFT_MIDDLE].servoPos.tibia & 0xff);
ax12write(leg[LEFT_MIDDLE].servoPos.tibia >> 8);
//LEFT FRONT
checksum += LF_COXA_ID + (leg[LEFT_FRONT].servoPos.coxa & 0xff) + (leg[LEFT_FRONT].servoPos.coxa >> 8);
ax12write(LF_COXA_ID);
ax12write(leg[LEFT_FRONT].servoPos.coxa & 0xff);
ax12write(leg[LEFT_FRONT].servoPos.coxa >> 8);
checksum += LF_FEMUR_ID + (leg[LEFT_FRONT].servoPos.femur & 0xff) + (leg[LEFT_FRONT].servoPos.femur >> 8);
ax12write(LF_FEMUR_ID);
ax12write(leg[LEFT_FRONT].servoPos.femur & 0xff);
ax12write(leg[LEFT_FRONT].servoPos.femur >> 8);
checksum += LF_TIBIA_ID + (leg[LEFT_FRONT].servoPos.tibia & 0xff) + (leg[LEFT_FRONT].servoPos.tibia >> 8);
ax12write(LF_TIBIA_ID);
ax12write(leg[LEFT_FRONT].servoPos.tibia & 0xff);
ax12write(leg[LEFT_FRONT].servoPos.tibia >> 8);
ax12write(0xff - (checksum % 256));
setRX(0);
}
The comments match the descriptions in the datasheet so you can follow along. Basically, the way my servos are ID’ed and leg array is numbered, I can’t cleanly loop over them and index. Maybe someday I’ll redo that. But for now this works well enough.
The last thing I thought about is the servo update rate. After some research i settled on a 30hz update rate or frame rate. This means that the main loop of my program needs to run every 33ms and every loop should call the ax12SyncWriteServos() function to update all the servo positions. This also implies that ALL my calculations for the frame need to be calculated in 33ms! (foreshadowing…)
So at this point I can give all my servos a unique angle and they’ll move to it. Brilliant!
On to Phase 2: Leg Inverse Kinematics, i.e., Pointing a Foot in 3D Space!
Or head back to the beginning of Project B.E.T.H.