Hey everyone,
I have encountered a very strange bug where sending a specific set of bytes over serial causes the Teensy to crash and no longer respond to any further serial commands. I am using a Teensy LC to control a Dynamixel Pro. The Dynamixel pro has a range of +2,147,483,648 to -2,147,483,648 counts so I am using 4 bytes to send the 32-bit long positional data. The breakdown for the serial packet for moving to position 0 is as follows:
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] [%] [#]
Any variation of this packet is sent from the PC to the Teensy without issue, except for when I send this specific packet:
36 255 253 255 255 5 37 35
($) [-131073 Counts] [Checksum] [%] [#]
This causes the program to no longer respond to further positional commands and requires a hard reset of the Teensy before commands can be sent again. Here is the stripped down code that handles all of this:
I don't really understand what part of this code is breaking. If anyone could help me to understand what I am doing wrong, I would really appreciate it!
I have encountered a very strange bug where sending a specific set of bytes over serial causes the Teensy to crash and no longer respond to any further serial commands. I am using a Teensy LC to control a Dynamixel Pro. The Dynamixel pro has a range of +2,147,483,648 to -2,147,483,648 counts so I am using 4 bytes to send the 32-bit long positional data. The breakdown for the serial packet for moving to position 0 is as follows:
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] [%] [#]
Any variation of this packet is sent from the PC to the Teensy without issue, except for when I send this specific packet:
36 255 253 255 255 5 37 35
($) [-131073 Counts] [Checksum] [%] [#]
This causes the program to no longer respond to further positional commands and requires a hard reset of the Teensy before commands can be sent again. Here is the stripped down code that handles all of this:
Code:
#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
long Goal_Position; // The goal position value of the Dynamixel
// 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.
{
//Do nothing
}
else if (Desired_Position != Goal_Position) // If the desired position is not the same as the goal position:
{
dxl.setGoalPosition(DXL_ID, Desired_Position); // Set the new goal position
}
}
}I don't really understand what part of this code is breaking. If anyone could help me to understand what I am doing wrong, I would really appreciate it!
