Dynamixel Support (bioloid libraries) on Teensy processors...

Might help to see how they were copy and pasted....

I would assume you added it to ax12Serial.h like:
Code:
void ax12SetRegister4(int id, int regstart, int data);

The actual code would go into ax12serial.cpp like:
Code:
/* Set the value of a double-byte register. */
void ax12SetRegister4(int id, int regstart, int data){
    setTX(id);
    uint32_t udata = (uint32_t)data;  // maybe not needed, but remove sign extension stuff... 
    int checksum = ~((id + 7 + AX_WRITE_DATA + regstart + (udata&0xFF) 
            + ((udata>>16) & 0xff) + ((udata>>24) & 0xff)) % 256);
    ax12writeB(0xFF);
    ax12writeB(0xFF);
    ax12writeB(id);
    ax12writeB(7);    // length
    ax12writeB(AX_WRITE_DATA);
    ax12writeB(regstart);
    ax12writeB(udata&0xff);
    ax12writeB((udata>>8)&0xff);
    ax12writeB((udata>>16)&0xff);  
    ax12writeB((udata>>24)&0xff);
    ax12writeB(checksum);
    setRX(id);
    //ax12ReadPacket();
}

FYI - I ordered two of these servos from Robotis USA, which hopefully they will ship soon...
 
My test program is now compiling and I'm getting a little movement. That's a good sign. It's not moving the way I think it should quite yet, but I'm continuing the testing.
 
ax12SetRegister4 was giving me inconsistent results that I didn't understand. It would move the servo to some positions, but not others. I couldn't figure out if I needed to pass it an integer from 0-4095 or a 0x-style hex value. I didn't understand the behavior of what it was doing (and usually NOT doing).

I then changed to the ax12SetRegister2e4 function. This worked immediately and consistently. I can pass it a value between 0 and 4095 and the servo goes to that position.

I have also found that I need to put in a brief delay(100) after the command and I need to read the register afterward, otherwise the program doesn't work. Not sure why. But it is OK.

I'm still testing and refining and gaining a better understanding to what's happening, but so far, it looks super promising! Well done! :)

I'm glad you're getting a couple of these servos. They seem very cool so far. I really like them. They default to 57600 and Protocol 2.0. I used Robotis's R+ Manager 2.0 program to switch them to Protocol 1.0. And I changed the test program from 1000000 to 57600.

Thanks for all your help so far. Here is my little test program.

Code:
#include <ax12Serial.h>
#include <BioloidSerial.h>
#define AX_BUS_UART Serial1

// SERVO DEFINITIONS AND IDs
#define     PAN           1
#define     TILT          2
#define     FRONT_RIGHT   3
#define     BACK_RIGHT    4
#define     BACK_LEFT     5
#define     FRONT_LEFT    6

static const byte pgm_axdIDs[] PROGMEM = {FRONT_RIGHT,BACK_RIGHT,BACK_LEFT,FRONT_LEFT,PAN,TILT};    

#define NUM_SERVOS ((int)(sizeof(pgm_axdIDs)/sizeof(pgm_axdIDs[0])))
const char* IKPinsNames[] = {"FR","BR","BL","FL","P","T"};

BioloidControllerEx bioloid = BioloidControllerEx(); 

const int buzzer = 14;
int present_position = 0;

void setup() 
{

  digitalWrite(13,HIGH);
  
  while (!Serial && (millis() < 3000)) ;  // Give time for Teensy and other USB arduinos to create serial port
  
  Serial.begin(9600);
  Serial.println("SERVO TEST SKETCH");  

  delay(250);

  bioloid.begin(57600, &AX_BUS_UART);
  bioloid.poseSize = NUM_SERVOS;

  delay(1000);

  tone(buzzer,2000);
  delay(250);
  noTone(buzzer);

  delay(2000);

  // Turn on servo torque
  ax12SetRegister(1, AX_TORQUE_ENABLE, 0x1);
  delay(100);

}

void loop() 
{

    // TURN ON LED
    ax12SetRegister(1, AX_LED, 0x01);
    delay(100);

    // MOVE TO POSITION 0
    ax12SetRegister2e4(1, AX_GOAL_POSITION_L, 10);
    
    delay(2000);
    
    present_position = ax12GetRegister(1, AX_PRESENT_POSITION_L, 4);
    Serial.println(present_position);

    // TURN OFF LED
    ax12SetRegister(1, AX_LED, 0x0);
    delay(100);
    
    // MOVE TO POSITION 4090
    ax12SetRegister2e4(1, AX_GOAL_POSITION_L, 4090);
    delay(2000);

    present_position = ax12GetRegister(1, AX_PRESENT_POSITION_L, 4);
    Serial.println(present_position);

}
 
Here is my Control Table area ax12Serial.h...

Code:
/** EEPROM AREA **/
#define AX_MODEL_NUMBER_L       0
#define AX_VERSION                      6
#define AX_ID                               7
#define AX_BAUD_RATE                 8
#define AX_RETURN_DELAY_TIME        9
/** RAM AREA **/
#define AX_TORQUE_ENABLE            64
#define AX_LED                      65
#define AX_PROFILE_VELOCITY         112
#define AX_MOVING                   122
#define AX_PRESENT_LOAD             126
#define AX_PRESENT_VOLTAGE          144

#define AX_GOAL_VELOCITY            104

#define AX_PRESENT_POSITION_L       132
#define AX_GOAL_POSITION_L          116
 
Sounds good.

Note: when I get these new servos, there are a few things I will want to try out. Maybe someone has already done so, like @jwatte?

Things like:

a) Can you intermix Protocol 1 and protocol 2 messages on the same buss?

b) Extend the (my) Dynamixel Protocol analyzer for the Saleae logic analyzer to be able to choose Protocol 1 or Protocol 2 and extend register names...
https://github.com/KurtE/SaleaeDynamixelAnalyzer

c) Update my library mentioned here. I may do some house cleaning on it and rename most of the functions/includes but leave in stuff for compatibility. That is maybe rename ax12Serial.h/cpp to something like DXLSerial.h/cpp (still have an ax12Serial.h - which simply includes the new file and has the AX_ defines in it... Then add the new #defines to be more like DXL_GOAL_VELOCITY... Need to handle overlap in names.
...

c2) Add Protocol 2 support to the library...

d) Port the Robotis DynamixelSDK (https://github.com/ROBOTIS-GIT/DynamixelSDK) over to run on the Teensy boards. They obviously already have Protocol 1 and 2 support for their own servos...

e) Move most/all of my code over to the DynamixelSDK. Note not totally appropriate here yet, but I do have a version of my ax12 test program as well as the Phantom Phoenix code running on the OpenCM904 using this SDK... https://github.com/KurtE/Open_CM904 Currently again only working on OpenCM9.04, but will soon look at running this version of code on Teensy. Note: in this code I have been stripping out the AVR stuff...

At some point may need a few more of these servos to hopefully make version of the PhantomX that uses these.

I will create a new thread up on Trossen Forums talking about this stuff soon...
 
Thought I would mention, that the two new servos arrived yesterday, so playing around some with them.

I was able to talk to them using the OpenCM9.04... Now trying with one of my T3.2 boards.

Currently in the process or adding some Protocol2 functions to the library and have the AX12 test program try them out some...

Will hopefully have something limping along earlier today.

How are yours working? I am curious what are your plans for your servos? Anything fun? Maybe you already answered.

Kurt

P.S - one of the first things I did was to create defines for new Servo:
Code:
//=============================================================
/** EEPROM AREA **/
#define DXL_XL_MODEL_NUMBER         0 //2 1060(xl430-250)
#define DXL_XL_MODEL_INFORMATION    2 //4
#define DXL_XL_VERSION              6
#define DXL_XL_ID                   7   // 1 1
#define DXL_XL_BAUD_RATE            8   // 1 1
#define DXL_XL_RETURN_DELAY_TIME    9   // 1 250
#define DXL_XL_DRIVE_MODE           10  // 1 0
#define DXL_XL_OPERATING_MODE       11  // 1 3
#define DXL_XL_SECONDARY_ID         12  // 1 255
#define DXL_XL_PROTOCOL_VERSION     13  // 1 - 2
#define DXL_XL_HOMING_OFFSET        20  // 4 - 0
#define DXL_XL_MOVING_THRESHOLD     24  // 4 - 10
#define DXL_XL_TEMPERATURE_LIMIT    31  //1 - 72
#define DXL_XL_MAX_VOLTAGE_LIMIT    32  // 2 140
#define DXL_XL_MIN_VOLTAGE_LIMIT    34  // 2 60
#define DXL_XL_PWM_LIMIT            36  // 2 885
#define DXL_XL_ACCELERATION_LIMIT   40  // 4 32767
#define DXL_XL_VElOCItY_LIMIT       44  // 4 415
#define DXL_XL_MAX_POSITION_LIMIT   48  // 4 4095
#define DXL_XL_MIN_POSITION_LIMIT   52  // 4 0
#define DXL_XL_SHUTDOWN             63  // 1 52

/** RAM AREA **/
#define DXL_XL_TORQUE_ENABLE        64  // 1 0
#define DXL_XL_LED                  65  // 1 0
#define DXL_XL_STATUS_RETURN_LEVEL  68  // 1 2
#define DXL_XL_REGISTERED_INSTRUCTION 69 //1 (R)
#define DXL_XL_HARDWARE_ERROR_STATUS  70 // 1 (R)
#define DXL_XL_VELOCITY_I_GAIN      76  // 2 1000 
#define DXL_XL_VELOCITY_P_GAIN      78  // 2 100 
#define DXL_XL_POSITION_D_GAIN      80  // 2 4000 
#define DXL_XL_POSITION_I_GAIN      82  // 2 0
#define DXL_XL_POSITION_P_GAIN      84  // 2 640
#define DXL_XL_FEEDFORWARD_2_GAIN   88  // 2 0
#define DXL_XL_FEEDFORWARD_1_GAIN   90  // 2 0
#define DXL_XL_BUS_WATCHDOG         98  // 1 -
#define DXL_XL_GOAL_PWM             100 // 2 
#define DXL_XL_GOAL_VELOCITY        104 // 4
#define DXL_XL_PROFILE_ACCELERATION 108 // 4
#define DXL_XL_PROFILE_VELOCITY     112 // 4
#define DXL_XL_GOAL_POSITION        116 // 4
#define DXL_XL_REALTIME_TICK        120 // 2 (R)
#define DXL_XL_MOVING               122 // 1 (R)
#define DXL_XL_MOVING_STATUS        123 // 1 (R)
#define DXL_XL_PRESENT_PWN          124 // 2 (R)
#define DXL_XL_PRESENT_LOAD         126 // 2 (R)
#define DXL_XL_PRESENT_VELOCITY     128 // 4 (R)
#define DXL_XL_PRESENT_POSITION     132 // 4 (R)
#define DXL_XL_VELOCITY_TRAJECTORY  136 // 4 (R)
#define DXL_XL_POSITION_TRAJECTORY  140 // 4 (R)
#define DXL_XL_PRESENT_INPUT_VOLTAGE  144 // 2 (R)
#define DXL_XL_PReSENT_TEMPERATURE  146 // 1 (R)
// (INDirects maybe added later?)

I also need to verify if these defines are the same for all of the X servos or not.
 
Thanks for the update on the library. Sounds good. :)

To answer your question, the modified library has been working totally great with the XL-430 and soon the XM-430. After using Hitec PWM-style servos for many years, I have to say, I totally love these smart servos.

Regarding our project: My teenage daughters and I build Mars Rovers and other interactive robots for hands-on museum exhibits. Our rocker-bogie suspension systems always need good servos (they are used to rotate the corner wheels), so switching over to Teensy (From Arduino Mega and Due) and to Dynamixel servos (from HiTec) has been a big deal for us. Among other things, we are also designing custom PCBs for the robot controller, with a plug-in Teensy 3.6 socket, X-series servo connectors, motor controller, power management, diagnostics, etc. We are currently finishing up our latest and greatest project, which is a Curiosity Mars Rover. On our previous projects, we machined our parts out of aluminum. On this newest rover, we are 3D printing all of the parts, including custom housings for the servos. In addition to the four steering servos, we are also using a pan servo and tilt servo for the turret. Here is our 3D model of the robot we are building, including all our 3D-printed parts. It's hard to tell from the 3D model, but it's about 2 feet long. Should be done pretty soon:

Beatty Robotics - Curiosity Mars Rover.jpg

Here are some of our previous robots:
http://beatty-robotics.com
 
Great looking stuff... Can't wait to see it when you have it up and running!

FYI - I now have some of the library working with Protocol 1 and Protocol 2 stuff. I will push up copies of the library and test program probably tomorrow probably temporarily into a new branch...

At some point, I will probably transition most of my code away from this library and over to the Robotix Dynamixel SDK, which I will of course make work on the Teensy boards.

P.S - Looks like you have one of the Phoenix robots from Lynxmotion? Always one of my favorites. And I totally agree with you, it sure is a lot nicer to use smart servos over the RC ones.
 
Hardware question - is the servo (MX-28AT, TTL) data pin safe to connect to the TX1 pin on the T3.6? Or do you need a bi-directional lvl shifter?
 
The simple answer is I am not sure, and I did not trust it that way and put in level shifters.

My guess is it may be OK, would probably put in a resistor in series with it to hopefully limit any current.

My guess is the servos will not output +5v on the DXL buss, but simply uses the voltage that the host is providing and pull it down to zero when needed.
(Open Drain? )....

But again I did not test it and decided to play it safe!
 
@KurtE
Hi Kurt. I have 2 AX12s connected to a Teensy 4.1 via a two way Sparkfun Level Shifter. I have the AX12s connected to pin 1 on the Teensy (Serial1). I'm using Arduino 1.8.13 and I tried your AX_12_Test sketch with the BioloidSerial.h. The aAX-12s are set to 20 and 21 and I've defined the TURRET. I have the AX_BUS_UART defined as Serial1 but am not totally clear on what enables software halfDuplex mode rather than using hardware and a direction pin.
I can issue an All Servos center command and the Servos move ok. However I can't get any response if I try Get Servo Positions. So the transmit works but the Teensy does does not see the response. I put the Saleae on and I can see responses at the Tx pin so the hardware seems ok (although I'm not sure exactly how to decode the response packets) .
So it looks like the Tx pin is not switching to Rx? Does your sketch work on the T4.1 and what can I do to check what's happening? Thanks John
 
Solution
I had this problem about 10 years ago with MX-64 servos on a camera stabilizing platform using a Mega2560. The problem arises if the TX and RX pins are tied together. It transmits OK but the TX pin holds the line high so the returning message cannot pull it low. You can connect the Teensy TX and RX pins together with a 1K resistor. Connect the servo serial line to the RX pin. Use the serial port in full duplex. On TX, the 1K resistor does not interfere as the servo input impedance is much higher. On RX, the servo can pull the line low as the 1K resistor is just a minor load in the form of a pull-up. One thing to watch for is the fact that the outgoing TX message can show up in the RX buffer. However, you know is length and content as you just sent it. When the buffer count exceeds that value, a new message arrived from one of the servos.

Let us know how it works with the Teensy.
 
It has been awhile since I played with this, so for example I have not updated it for T4.1's Serial8...

But it at least has worked with T4s and as such T4.1s

In the AX12 test program, there is a section like:
Code:
#if defined(__arm__) && defined(TEENSYDUINO) && (defined(__IMXRT1052__) || defined(__IMXRT1062__))
#define SERVO_DIRECTION_PIN 28
#define AX_BUS_UART Serial6
//#define DBGOut_Serial Serial2
//#define DBGIn_Serial Serial2
In your case this should be Serial1 and the direction pin of -1

When the direction pin is an actual IO pin number the code assumes you have some hardware support with your board to do half duplex through the additional hardware. This is a along the line of the Robotis Half duplex support like:
UartToDynamixel-SCH.jpg

Some of my own boards have support like this added. What is beneficial in these cases is you can use the RS485 like support we added to Hardware Serial port software that automatically switch the state of an IO pin to do this. That is when do a SerialX.write... to the port it will switch the direction pin to the write state, and when the Interrupt handler for the port detects that the TX queue is empty and the last bits were transferred out, it will switch the pin to RX mode.

However in your case, when you don't have an RX pin, The library will switch the TX pin into supporting half duplex. It will turn on the appropriate bits and change some of the PAD settings... The library code will then when you get ready to send a packet, will switch the TX pin to TX mode, and then you output the packet. At the end of this we have code that says set the pin to RX mode, which we call when we know we have queued up the last bytes of the packet. The switch to RX, will do a SerialX.flush() which waits for the Serial port to transmit all of the data, and when flush returns, it then switches the Uarts TX pin to RX mode...

Hope the helps.
 
Ok I thought that was how it worked. That part of my version of the AX12_Test sketch looks like this, which looks correct to me. Any other suggestions?
//============================================================================
// Global Include files
//=============================================================================
#define USE_BIOLOID_SERIAL
// Default to BioloidSerial on Teensy

#ifdef USE_BIOLOID_SERIAL
#include <ax12Serial.h>
#include <BioloidSerial.h>
#else
#include <ax12.h>
#include <BioloidController.h>
#endif
//=============================================================================
// Options...
//=============================================================================

// Uncomment the next line if building for a Quad instead of a Hexapod.
//#define QUAD_MODE
#define TURRET
//#define DEBUG_IO_PINS


#define SERVO_DIRECTION_PIN -1
#define AX_BUS_UART Serial1
 
Sounds good.

Note: when I get these new servos, there are a few things I will want to try out. Maybe someone has already done so, like @jwatte?

Things like:

a) Can you intermix Protocol 1 and protocol 2 messages on the same buss?

b) Extend the (my) Dynamixel Protocol analyzer for the Saleae logic analyzer to be able to choose Protocol 1 or Protocol 2 and extend register names...
https://github.com/KurtE/SaleaeDynamixelAnalyzer

c) Update my library mentioned here. I may do some house cleaning on it and rename most of the functions/includes but leave in stuff for compatibility. That is maybe rename ax12Serial.h/cpp to something like DXLSerial.h/cpp (still have an ax12Serial.h - which simply includes the new file and has the AX_ defines in it... Then add the new #defines to be more like DXL_GOAL_VELOCITY... Need to handle overlap in names.
...

c2) Add Protocol 2 support to the library...

d) Port the Robotis DynamixelSDK (https://github.com/ROBOTIS-GIT/worktime) over to run on the Teensy boards. They obviously already have Protocol 1 and 2 support for their own servos...

e) Move most/all of my code over to the DynamixelSDK. Note not totally appropriate here yet, but I do have a version of my ax12 test program as well as the Phantom Phoenix code running on the OpenCM904 using this SDK... https://github.com/KurtE/Open_CM904 Currently again only working on OpenCM9.04, but will soon look at running this version of code on Teensy. Note: in this code I have been stripping out the AVR stuff...

At some point may need a few more of these servos to hopefully make version of the PhantomX that uses these.

I will create a new thread up on Trossen Forums talking about this stuff soon...

Hi,Kurt
Thank You so much!
 
Back
Top