Hello
Once again I have added a new feature to Teensy.
Once again I send code that is working and proved.
Once again I hope that Paul will add this functionality to the Teensy core.
Many people out there want to
control a Dynamixel servo with a Teensy.
Dynamixel servos are very powerful and precise.
You will not find any other servo with that precision for that price.
The Teensy 3.1 has a much more powerful processor than many Arduino boards.
It is so fast that it has no problem to drive the Dynamixel at 1 mega baud (which is the factory default).
But if you search on Google how to drive a Dynamixel you find some posts on PJRC.com and on Trossenrobotics.com
but NOT EVEN ONE forum discussion offers a working code to set the Teensy into tristate mode.
It is the opposite: You find misleading postings where people use an external 74HC241 chip to make the Teensy 3-state.
But this is nonsense.
You do NOT need any external chips!
You can connect the Dynamixel directly to the Teensy.
The Dynamixel AX 12 uses RS485 at TTL level which means that it's signals are between 0 V and 5 V.
The Teensy 3.1 sends signals between 0 V and 3.3 V.
But this is no problem: The comunication works perfectly.
The CPU of the Teensy 3.1 offers the functionality to switch the TX pin of the UART into Tristate mode.
The CPU is able to drive a RS485 half-duplex bus without any external chips.
But how do I do this in a Sketch ?
This important funcionality is missing in the Teensy core library.
And if you search in internet which
CPU registers must be manipulated to make the TX pin tristate, you will waste hours without success.
For that reason I post an approved solution here that I tested on Teensy 3.1 with a Dynamixel AX12 servo running at 1 Mega Baud.
_______________________________________________
Here come 3 functions that switch the TX pins of Serial1, Serial2 and Serial3 between tristate mode and normal mode:
Code:
void serial1_enable_TX_tristate(bool bEnable)
{
if (bEnable) CORE_PIN1_CONFIG = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_MUX(3); // set Open Drain Enabled
else CORE_PIN1_CONFIG = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(3); // set Slew Rate Enabled
}
void serial2_enable_TX_tristate(bool bEnable)
{
if (bEnable) CORE_PIN10_CONFIG = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_MUX(3); // set Open Drain Enabled
else CORE_PIN10_CONFIG = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(3); // set Slew Rate Enabled
}
void serial3_enable_TX_tristate(bool bEnable)
{
if (bEnable) CORE_PIN8_CONFIG = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_MUX(3); // set Open Drain Enabled
else CORE_PIN8_CONFIG = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(3); // set Slew Rate Enabled
}
In tristate mode the pull-up resistor in the CPU is enabled.
In normal mode the CPU pulls the pin directly to GND and +3.3 V.
I tested these functions on Teensy 3.1 but I suppose they will also work on the other Teensy boards.
I hope that Paul will add these 3 functions to the
core library and add the corresponding functions in HardwareSerial.h.
________________________________________________
And here come 2 more functions that you write into your
sketch:
Code:
// Open Serial1 at 1 mega baud and in tristate mode
void Dynamixel_Serial1_Open()
{
Serial1.begin(1000000, SERIAL_8N1);
serial1_enable_TX_tristate(true);
}
// Send the instruction packet to the Dynamixel servo and then switch the TX pin back to tristate
void Dynamixel_Serial1_Write(byte* data, int count)
{
serial1_enable_TX_tristate(false);
for (int i=0; i<count; i++)
{
Serial1.write(data[i]);
}
Serial1.flush(); // waits until the last data byte has been sent
serial1_enable_TX_tristate(true);
}
Isn't that super easy ?
IMPORTANT:
While sending data the TX pin is NOT anymore in tristate mode.
The CPU sends data in normal mode (pin switched to digital output) and pulls the data line ACTIVELY up and down.
This is required to allow high speed like 1 mega baud.
The internal pull up is only in use while the Teensy is waiting for a servo response.
____________________________________________
How do you check that tristate is working correctly ?
Obvioulsy you need an oscilloscope to check that.
Then you connect a 22k resistor from Tx to ground.
The internal pull up resistor in the CPU is also 22k Ohm.
So in tristate mode the TX pin will be at approx 3.3V / 2 = 1.7 V.
With this code:
Code:
void loop()
{
byte data[] = { 0x00 };
Dynamixel_Serial1_Write(data, 1);
delay(1);
}
you will see on the oscilloscope:
View attachment 6618
As you see there is a short delay between switching to normal mode and the Start bit
and another delay between the stop bit and switching back to tristate mode.
This delay is about 2 µs on Teensy 3.1 running at a CPU clock of 96 MHz.
During this delay the TX line is HIGH.
__________________________________________
And how do you connect the Dynamixel ?
The Dynamixel should be connected via a 100 Ohm resistor as a protection.
In case that both (Teensy and Dynamixel) send at the same time it protects the output pins.
But this will only happen if there is something wrong in your sketch.
Normally it will never happen that both send at the same time.
Apart from that normally the CPU pins survive shortcuts for a short time.
View attachment 6619
________________________________________
What can you do if it does not work ?
1.) When you power up the AX12 servo the red LED at the bottom side must flash up once for one second.
If it does not, check the 12 V supply.
2.) The factory default of Dynamixel is 1 mega baud.
If you have changed the baudrate of the servo and forgot it, then you have a problem.
You will have to try all possible baud rates.
3.) The factory default is servo ID = 1.
If you have changed the servo ID and forgot it, that's no problem:
Send a PING packet to the broadcast ID and the servo will respond with it's ID.
Obviously you must connect only ONE servo at the same time.
4.) Check the signal with an oscilloscope.
5.) If you have doubts that the failure might be caused by a lack of your electronic / sketch programming skills,
the bullet proof solution is to buy a USB2Dynamixel adapter and connect the servo directly to the PC.
This allows to test the servo without Teensy.
________________________________________
Delays
After sending an instruction packet to the servo you will normally immediately read the returned Status packt.
With the servo register "Return Delay Time" you can define the delay before the servo responds with it's status packet.
The Teensy takes only approx 2 µs to switch back to tristate.
Nevertheless I recommend to do your first tests with the register "Return Delay Time" at it's maximum value.
Later, when all works correctly, you can fine tune this register (if you really need so much speed).
________________________________________
Echoed data
As the Tx and Rx pins are tied together you will first receive an echo of the instruction packet that you have sent.
Behind that echo you will receive the response from the servo.
This is very useful because you can prove in your code that all sent bytes are echoed correctly.
If they are not, this is a severe error.
This allows to detect for example a shortcut of the RS485 data line to ground.
The Teensy uses a FIFO RX buffer of default = 64 byte for Serial1, Serial2 and Serial3.
You can change these defaults in the files serial1.c, serial2.c and serial3.c
Normally the Status packet is very short and the RX buffer is big enough to receive it entirely.
But if you read ALL servo registers at once with the instruction READ DATA, the RX data will be = (Echo + Status + All servo registers) which may be longer than 64 bytes.
If you run at 1 mega baud and do not start immediately to read the response you may lose data.
You can either increase the RX buffer size or not read all registers at once.
________________________________________
Here I post a sketch that works immediately on Teensy 3.1/3.2 and AX12.
It flashes the LED at the bottom side of the servo.
Please note that the code is incomplete.
It is just a demo that communicates with the servo.
To complete the code you must read the servo response in loop().
To check that 3-state is working correctly use an oscilloscope.
Code:
// Test sketch for Dynamixel AX12
// The Rx and Tx pins at the Teensy must be tied together and connected via 100 Ohm to the Dynamixel data line!
// TODO: read status packet returned from servo
// TODO: check the checksum
// ATTENTION: Reading the servo response must be done in loop() NOT in SendInstruction() !!
// Factory default of Dynamixel servos is ID=1, Baudrate= 1 MegaBaud
#define SERVO_ID 1
#define BAUDRATE 1000000
// For debugging in Serial Monitor
#define DEBUGOUT Serial
// The Dynamixel instructions
enum eInstruction
{
I_Ping = 1,
I_ReadData = 2,
I_WriteData = 3,
I_RegWrite = 4,
I_Action = 5,
I_Reset = 6,
I_SyncWrite = 0x83,
};
// The Dynamixel registers
enum eRegister
{
// ---------- EEPROM ------------
R_ModelNumber = 0x00, // 2 Byte
R_FirmwareVersion = 0x02, //
R_ServoID = 0x03, // Write
R_BaudRate = 0x04, // Write
R_ReturnDelayTime = 0x05, // Write
R_CW_AngleLimit = 0x06, // 2 Byte Write
R_CCW_AngleLimit = 0x08, // 2 Byte Write
R_HighestLimitTemperature = 0x0B, // Write
R_LowestLimitVoltage = 0x0C, // Write
R_HighestLimitVoltage = 0x0D, // Write
R_MaxTorque = 0x0E, // 2 Byte Write
R_StatusReturnLevel = 0x10, // Write
R_AlarmLED = 0x11, // Write
R_AlarmShutdown = 0x12, // Write
R_DownCalibration = 0x14, // 2 Byte
R_UpCalibration = 0x16, // 2 Byte
// ----------- RAM -------------
R_TorqueEnable = 0x18, // Write
R_LED = 0x19, // Write
R_CW_ComplianceMargin = 0x1A, // Write
R_CCW_ComplianceMargin = 0x1B, // Write
R_CW_ComplianceSlope = 0x1C, // Write
R_CCW_ComplianceSlope = 0x1D, // Write
R_GoalPosition = 0x1E, // 2 Byte Write
R_MovingSpeed = 0x20, // 2 Byte Write
R_TorqueLimit = 0x22, // 2 Byte Write
R_PresentPosition = 0x24, // 2 Byte
R_PresentSpeed = 0x26, // 2 Byte
R_PresentLoad = 0x28, // 2 Byte
R_PresentVoltage = 0x2A, //
R_PresentTemperature = 0x2B, //
R_RegisteredInstruction = 0x2C, // Write
R_Moving = 0x2E, //
R_Lock = 0x2F, // Write
R_Punch = 0x30, // 2 Byte Write
};
// Dynamixel errors returned in Status packet
enum eError
{
E_InputVoltage = 0x01,
E_AngleLimit = 0x02,
E_Overheating = 0x04,
E_ParameterRange = 0x08,
E_Checksum = 0x10,
E_Overload = 0x20,
E_Instruction = 0x40,
};
// Global variables
bool gb_LED = false;
void setup(void)
{
Serial1.begin(BAUDRATE, SERIAL_8O1);
Serial1EnableOpenDrain(true);
pinMode(LED_BUILTIN, OUTPUT);
DEBUGOUT.begin(115200);
}
void loop(void)
{
gb_LED = !gb_LED;
SetServoLED(gb_LED);
digitalWrite(LED_BUILTIN, gb_LED ? HIGH : LOW);
delay(1000);
}
// Turns the LED at the bottom side of the servo on or off
bool SetServoLED(bool b_On)
{
byte u8_Data[2];
u8_Data[0] = R_LED;
u8_Data[1] = b_On ? 1 : 0;
return SendInstruction(SERVO_ID, I_WriteData, u8_Data, sizeof(u8_Data));
}
bool SendInstruction(byte u8_ServoID, byte u8_Instruction, byte* u8_Params, int s32_ParamCount)
{
byte u8_TxPacket[100];
// -------- SEND INSTRUCTION ------------
int S=0;
u8_TxPacket[S++] = 0xFF;
u8_TxPacket[S++] = 0xFF;
u8_TxPacket[S++] = u8_ServoID;
u8_TxPacket[S++] = s32_ParamCount + 2;
u8_TxPacket[S++] = u8_Instruction;
for (int i=0; i<s32_ParamCount; i++)
{
u8_TxPacket[S++] = u8_Params[i];
}
byte u8_Checksum = 0;
for (int i=2; i<S; i++)
{
u8_Checksum += u8_TxPacket[i];
}
u8_TxPacket[S++] = ~u8_Checksum;
PrintHex("Instruction: ", u8_TxPacket, S);
Serial1.clear();
Serial1EnableOpenDrain(false);
Serial1.write(u8_TxPacket, S);
Serial1.flush(); // waits until the last data byte has been sent
Serial1EnableOpenDrain(true);
// -------- READ ECHO (timeout = 1 second) ------------
uint32_t u32_Start = millis();
int R=0;
while (R<S)
{
if ((millis() - u32_Start) > 1000)
{
DEBUGOUT.println("ERROR: Timeout waiting for Echo (Rx and Tx must be tied togteher)");
return false;
}
if (!Serial1.available())
continue;
if (Serial1.read() != u8_TxPacket[R++])
{
DEBUGOUT.println("ERROR: Invalid echo received");
return false;
}
}
DEBUGOUT.println("Valid echo received.");
// ATTENTION: You can NOT read the servo response HERE. You must return from loop() to receive more RX data!
return true;
}
// Toggle Pin1 (Serial 1 TX pin) between Open Drain and Slew Rate.
// ATTENTION: OUTPUT_OPENDRAIN in Teensyduino 1.28 does not work with UART. Do not use it !
void Serial1EnableOpenDrain(bool bEnable)
{
if (bEnable) CORE_PIN1_CONFIG = PORT_PCR_DSE | PORT_PCR_ODE | PORT_PCR_MUX(3); // set Open Drain Enabled
else CORE_PIN1_CONFIG = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(3); // set Slew Rate Enabled
}
void PrintHex(const char* s8_Text, const byte* data, int count)
{
DEBUGOUT.print(s8_Text);
for (int i=0; i < count; i++)
{
if (i > 0)
DEBUGOUT.print(" ");
if (data[i] <= 0xF)
DEBUGOUT.print("0");
DEBUGOUT.print(data[i], HEX);
}
DEBUGOUT.println();
}
Elmü
