Hey everyone,
I am having a very specific and very obscure problem. I am trying to use a Teensy LC Microcontroller to control various different Dynamixels, but in this case a Dynamixel Pro. I am using a SP485EN-L transceiver to communicate between the two devices.
If I tell the Dynamixel to move to position -131073, it causes the Dynamixel to report an error and to report that it is at position 0. If I move it to 0, it reports that it is already there, and throws this error 10. Interestingly enough, if I ask it to move to another position other than 0, such as 150000, it moves just fine and the error is cleared.
This ONLY happens with -131073, other positions such as -131072 and -131074 do not exhibit this problem.
I am using the following serial packet to communicate with the Dynamixel from the PC:
36 0 0 0 0 0 37 35
($) [Positional Data] [Checksum] [%] [#]
another example of some valid data would be for position 150000
36 0 2 73 240 196 37 35
($) [Positional Data] [Checksum] [%] [#]
This is -131073 represented as a serial packet:
36 255 253 255 255 5 37 35
($) [-131073 Counts] [Checksum] [%] [#]
Even with the values hardcoded into the software, in this case of a dummy serial string of $000000#, it still causes the issue.
I hope that this all makes sense. I have no idea where the problem lies in this code, but hopefully I can solve it.
#include <Dynamixel2Arduino.h> // Official Dynamixel control library for Arduino
#define DXL_SERIAL Serial1 // Serial Port used to communicate with Dynamixel
#define PC_SERIAL Serial // Serial Port used to communicate with the PC
using namespace ControlTableItem; //This namespace is required to use Control table item names. Required for the Dynamixel Library.
int PC_numBytes; // Number of bytes that are ACTUALLY at the serial port (PC)
int PC_Expected_Bytes = 8; // Number of bytes that SHOULD be at the serial port (PC)
uint8_t DXL_ID = 1; // The ID of the Dynamixel
Dynamixel2Arduino dxl(DXL_SERIAL, 2); // Setup the Dynamixel Serial Port, Pin 2 for Transciever DIR Pin
void setup() {
dxl.begin(57600); // Initialize communications with the Dynamixel at 57600 baud
dxl.setPortProtocolVersion(2.0); // Set Port Protocol Version. This has to match with the DYNAMIXEL protocol version.
PC_SERIAL.begin(115200); // Initialize communications with the PC at 115200 baud
while(!PC_SERIAL) delay(10); // Only start sending data 10ms after the serial port is open
dxl.torqueOff(DXL_ID); // Turn the Torque Off. Prerequisite for changing EEPROM data.
dxl.torqueOn(DXL_ID); // Re-enable the torque
}
void loop() {
PC_numBytes = PC_SERIAL.available(); // Check to see how many bytes are waiting at the serial port.
if (PC_numBytes >= 1) // If there is something at the serial port:
{
// Peek to see if first character is the dollar sign. If not, clear the buffer. The $ indicates valid data.
if (PC_SERIAL.peek() != '$')
{
while (PC_SERIAL.available() > 0) PC_SERIAL.read(); // Flush serial RX buffer by reading the data
}
}
if (PC_numBytes >= PC_Expected_Bytes) // If there are the correct number of bytes waiting, then:
{
Serial_Parse(PC_Expected_Bytes); // run the Serial_Parse Function
}
}
void Serial_Parse(int Bytes)
{
char PC_Rx_Sentence[9] = "$000000#"; // Initialize received serial sentence (PC, 8 bytes)
long Desired_Position; // The desired position of the Dynamixel, either valid or invalid
long Goal_Position; // The goal position value of the Dynamixel
bool Success;
// Parse received serial
for (int x=0; x < Bytes; x++) //Put each byte received into an array
{
PC_Rx_Sentence[x] = PC_SERIAL.read();
}
// If the command starts with the $ sign, ends with the # sign, and the checksum matches (Same checksum style as Dynamixel) do this:
if (PC_Rx_Sentence[0] == '$' && PC_Rx_Sentence[Bytes - 1] == '#' && PC_Rx_Sentence[5] == lowByte(~(PC_Rx_Sentence[1] + PC_Rx_Sentence[2] + PC_Rx_Sentence[3] + PC_Rx_Sentence[4])) && PC_Rx_Sentence[6] == '%')
{
Desired_Position = (PC_Rx_Sentence[1] << 24) | (PC_Rx_Sentence[2] << 16) | ( PC_Rx_Sentence[3] << 8 ) | (PC_Rx_Sentence[4]); // This is how you make a 32-Bit number with four bytes
Goal_Position = (dxl.readControlTableItem(GOAL_POSITION, DXL_ID)); // Read the current goal position from the Dynamixel
if (Desired_Position == Goal_Position) // If the desired position is the same as the goal position, don't write anything, as nothing has changed.
{
PC_SERIAL.print("Error ");
PC_SERIAL.println((dxl.getLastLibErrCode()));
PC_SERIAL.print("Hardware Error Status: ");
PC_SERIAL.println(dxl.readControlTableItem(HARDWARE_ERROR_STATUS, DXL_ID));
PC_SERIAL.print("Dynamixel Model is: ");
PC_SERIAL.println(dxl.readControlTableItem(MODEL_NUMBER, DXL_ID));
PC_SERIAL.print("Dynamixel is already at ");
PC_SERIAL.print(Goal_Position);
PC_SERIAL.println("\r\n");
}
else if (Desired_Position != Goal_Position) // If the desired position is not the same as the goal position:
{
PC_SERIAL.print("Error ");
PC_SERIAL.println((dxl.getLastLibErrCode()));
PC_SERIAL.print("Hardware Error Status: ");
PC_SERIAL.println(dxl.readControlTableItem(HARDWARE_ERROR_STATUS, DXL_ID));
PC_SERIAL.print("Dynamixel Model is: ");
PC_SERIAL.println(dxl.readControlTableItem(MODEL_NUMBER, DXL_ID));
Success = dxl.setGoalPosition(DXL_ID, Desired_Position); // Set the new goal position
PC_SERIAL.print("Dynamixel is going to ");
PC_SERIAL.println(Desired_Position);
PC_SERIAL.print("Success or Failure? ");
PC_SERIAL.print(Success);
PC_SERIAL.println("\r\n");
}
}
else if (PC_Rx_Sentence[0] == '$' && PC_Rx_Sentence[Bytes - 1] == '#' && PC_Rx_Sentence[1] == '0' && PC_Rx_Sentence[2] == '0' && PC_Rx_Sentence[3] == '0' && PC_Rx_Sentence[4] == '0' && PC_Rx_Sentence[5] == '0' && PC_Rx_Sentence[6] == '0')
{
PC_SERIAL.print("Error ");
PC_SERIAL.println((dxl.getLastLibErrCode()));
PC_SERIAL.print("Hardware Error Status: ");
PC_SERIAL.println(dxl.readControlTableItem(HARDWARE_ERROR_STATUS, DXL_ID));
PC_SERIAL.print("Dynamixel Model is: ");
PC_SERIAL.println(dxl.readControlTableItem(MODEL_NUMBER, DXL_ID));
Success = dxl.setGoalPosition(DXL_ID, -131073); // Set the new goal position
PC_SERIAL.print("Dynamixel is going to ");
PC_SERIAL.println(-131073);
PC_SERIAL.print("Success or Failure? ");
PC_SERIAL.print(Success);
PC_SERIAL.println("\r\n");
}
else if (PC_Rx_Sentence[0] == '$' && PC_Rx_Sentence[Bytes - 1] == '#' && PC_Rx_Sentence[1] == '1' && PC_Rx_Sentence[2] == '1' && PC_Rx_Sentence[3] == '1' && PC_Rx_Sentence[4] == '1' && PC_Rx_Sentence[5] == '1' && PC_Rx_Sentence[6] == '1')
{
PC_SERIAL.print("Error ");
PC_SERIAL.println((dxl.getLastLibErrCode()));
PC_SERIAL.print("Hardware Error Status: ");
PC_SERIAL.println(dxl.readControlTableItem(HARDWARE_ERROR_STATUS, DXL_ID));
PC_SERIAL.print("Dynamixel Model is: ");
PC_SERIAL.println(dxl.readControlTableItem(MODEL_NUMBER, DXL_ID));
PC_SERIAL.print("Dynamixel is currently at ");
PC_SERIAL.print(Goal_Position);
PC_SERIAL.println("\r\n");
}
}