Hello everyone,
I'm new here and with Arduino, so I don't know it is right place to open this topic. But I think that my problem is about programming, especially about interrupts. My problem is about communication via Serial port on Arduino Mega 2560. In my project, I use Dynamixel RX-24F Servo motor and I use Arduino to control this motor. And I use a library (Dynamixel_Serial_V2_2.zip) for this purpose. In my project I need to move the motor between two points continuously (i.e., motor moves from point A to point B, then it move from point B to point A and it goes like this). To accomplish this, I used servo() function in library and I put delay() between two servo() function to wait the motor until it reaches the desired point. However, with this code I couldn't run the motor with the way I wanted. So I realized the caveat written in reference page of delay() function.
Then I changed my code by using millis() code instead of delay(). And it worked perfectly. However, now I am encountering a new problem. When I try to use readPosition() function in every seconds (or any time interval greater than 10 ms again by using millis() function) I cannot read the position. When I check the direction control pin (since Dynamixel uses half duplex communication we need to use a control pin to control TX/RX direction) with the oscilloscope I realized that control pin goes high and goes low immediately when I am using readPosition() function with some delay. It has to remain high while Arduino transmits data. And it remains high when I use servo() function with some delay. And if I don't put any delay between two readPosition() functions control pin remains high while transmission is continuing and I can read the position. And I think it doesn't make any sense because both function use the same private function transmitInstructionPacket() to transmit the packet via serial channel. Functions other than read functions (servo(), ping() etc.) works very well with some delay, i.e., control pin remains high during transmission.
I'm putting transmitInstructionPacket() and readStatusPacket() functions, which are used by other functions, servo() and readPosition() functions at the end. I am sorry about long post and complex explanation but I tried to be specific as much as I can. And I am sorry about possible grammatical errors since I am not native in English. I will be grateful if you can help me to figure out this problem.
Regards,
Mansur
transmitInstructionPacket() and readStatusPacket() functions.
void DynamixelClass::transmitInstructionPacket(void) // Transmit instruction packet to Dynamixel
{
unsigned char Counter;
Counter = 0;
digitalWrite(Direction_Pin,HIGH); // Set TX Buffer pin to HIGH
Serial1.write(HEADER); // Write Header (0xFF) data 1 to serial
Serial1.write(HEADER); // Write Header (0xFF) data 2 to serial
Serial1.write(Instruction_Packet_Array[0]); // Write Dynamixal ID to serial
Serial1.write(Instruction_Packet_Array[1]); // Write packet length to serial
do // Write Instuction & Parameters (if there is any) to serial
{
Serial1.write(Instruction_Packet_Array[Counter + 2]);
Counter++;
}while((Instruction_Packet_Array[1] - 2) >= Counter);
Serial1.write(Instruction_Packet_Array[Counter + 2]); // Write check sum to serial
if ((UCSR0A & B01100000) != B01100000) // Wait for TX data to be sent
{
Serial1.flush();
}
digitalWrite(Direction_Pin,LOW); //Set TX Buffer pin to LOW after data has been sent
}
unsigned int DynamixelClass::readStatusPacket(void)
{
unsigned char Counter = 0x00;
unsigned char First_Header = 0x00;
Status_Packet_Array[0] = 0x00;
Status_Packet_Array[1] = 0x00;
Status_Packet_Array[2] = 0x00;
Status_Packet_Array[3] = 0x00;
Time_Counter = STATUS_PACKET_TIMEOUT + millis(); // Setup time out error
while(STATUS_FRAME_BUFFER >= Serial1.available()) // Wait for " header + header + frame length + error " RX data
{
if (millis() >= Time_Counter)
{
return Status_Packet_Array[2] = B10000000; // Return with Error if Serial data not received with in time limit
}
}
if (Serial1.peek() == 0xFF && First_Header != 0xFF)
{
First_Header = Serial1.read(); // Clear 1st header from RX buffer
}
else if (Serial1.peek() == -1)
{
return Status_Packet_Array[2] = B10000000; // Return with Error if two headers are not found
}
if(Serial1.peek() == 0xFF && First_Header == 0xFF)
{
Serial1.read(); // Clear 2nd header from RX buffer
Status_Packet_Array[0] = Serial1.read(); // ID sent from Dynamixel
Status_Packet_Array[1] = Serial1.read(); // Frame Length of status packet
Status_Packet_Array[2] = Serial1.read(); // Error byte
Time_Counter = STATUS_PACKET_TIMEOUT + millis();
while(Status_Packet_Array[1] - 2 >= Serial1.available()) // Wait for wait for "Para1 + ... Para X" received data
{
if (millis() >= Time_Counter)
{
return Status_Packet_Array[2] = B10000000; // Return with Error if Serial data not received with in time limit
}
}
do
{
Status_Packet_Array[3 + Counter] = Serial1.read();
Counter++;
}while(Status_Packet_Array[1]-2 > Counter); // Read Parameter(s) into array
Status_Packet_Array[Counter + 3] = Serial1.read(); // Read Check sum
}
else
{
return Status_Packet_Array[2] = B10000000; // Return with Error if two headers are not found
}
}
servo() function (I changed the name of it but remain is the same with the original)
unsigned int DynamixelClass::joint(unsigned char ID, unsigned int position, unsigned int speed)
{
Instruction_Packet_Array[0] = ID;
Instruction_Packet_Array[1] = JOINT_LENGTH;
Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
Instruction_Packet_Array[4] = byte(position);
Instruction_Packet_Array[5] = byte((position & 0x0F00) >> 8);
Instruction_Packet_Array[6] = byte(speed);
Instruction_Packet_Array[7] = byte(speed >> 8);
Instruction_Packet_Array[8] = ~(ID + JOINT_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + byte(position) + byte((position & 0x0F00) >> 8) + byte(speed) + byte(speed >> 8));
while (Serial1.read() != -1); // Clear RX buffer;
transmitInstructionPacket();
if (ID == 0xFE || Status_Return_Value != ALL ) // If ID of FE is used no status packets are returned so we do not need to check it
{
return (0x00);
}
else
{
readStatusPacket();
if (Status_Packet_Array[2] == 0) // If there is no status packet error return value
{
return (Status_Packet_Array[0]); // Return SERVO ID
}
else
{
return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
}
}
}
readPosition() function
unsigned int DynamixelClass::readPosition(unsigned char ID)
{
Instruction_Packet_Array[0] = ID;
Instruction_Packet_Array[1] = READ_LENGTH;
Instruction_Packet_Array[2] = COMMAND_READ_DATA;
Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L;
Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
Instruction_Packet_Array[5] = ~(ID + READ_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH);
while (Serial1.read() != -1); // Clear RX buffer;
transmitInstructionPacket();
readStatusPacket();
if (Status_Packet_Array[2] == 0)
{
return Status_Packet_Array[4] << 8 | Status_Packet_Array[3]; // Return present position value
}
else
{
return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
}
}
I'm trying PART 1 and PART 2 separately but I put them together here.