Hello Arduino community!
I am reaching out today as I need some help with a project that I am working on.
At a high level, I have been working with a servo motor that I would like to use in position and torque control. I thought of Arduino to be one of the best mediums to achieve this, for both information processing, control, and prototyping.
The components involved in the setup include;
-
Servo Motor
-
Servo Drive which utilizes RS232 Torque Control
-
Arduino Mega 2560
I purchased the motor and drive from a company that already had a pre-existing library that included both Position and Torque Control in Arduino. However, upon troubleshooting I quickly realized that I was unable to engage Torque Control. The code compiles and runs with no bugs. I have tried multiple techniques and different ways to try and engage torque control with little to no success. This why I thought I would reach out the community to help me figure out how I can achieve this and perhaps spot some inconsistencies with the library and packages.
The Torque – Position switch theoretically exists with the function provided below.
Command Mode Switch
Function Code = General Read 0x0E
4 Byte data:
B0 : Command mode switch 0x0D
B1 : 1 = Write 2 = Read (CMD)
B2 : Data B0 (always 0)
B3 : Data B1 (See below)
Change to Torque Mode B3 : Data B1 = 2
Change to Position Mode B3 : Data B1 = 1
*/
/*********************************************************
long RS232_TQ_CMD_SETREAD_MODE(unsigned char CMD, unsigned char DATA)
{
long Command;
Command = 0x0D;
Command<<=7; // 7 because the DMM RS232 data format shifts by 7
Command &=0x3f80; // clear bottom 7 bits
Command += CMD;
Command<<=7;
Command &=0x1FFF80; // clear bottom 7 bits
Command += DATA;
char Axis_Num = global_CtdDriveID; //Chnage to user axis num
Global_Func = (char)General_Read;
Send_Package(Axis_Num, Command);
Sleep(10);
if(CMD==2)
{ // if command is read command, read the data back
ReadPackage();
return TRQMODE_READ_DATA;
}
}
To provide further context, this command utilizes the Send_Package Function, which I will provide below;
void Send_Package(char ID , long Displacement) {
unsigned char B[8], Package_Length, Function_Code;
long TempLong;
B[1] = B[2] = B[3] = B[4] = B[5] = (unsigned char)0x80;
B[0] = ID & 0x7f;
Function_Code = Global_Func & 0x1f;
TempLong = Displacement & 0x0fffffff; //Max 28bits
B[5] += (unsigned char)TempLong & 0x0000007f;
TempLong = TempLong >> 7;
B[4] += (unsigned char)TempLong & 0x0000007f;
TempLong = TempLong >> 7;
B[3] += (unsigned char)TempLong & 0x0000007f;
TempLong = TempLong >> 7;
B[2] += (unsigned char)TempLong & 0x0000007f;
Package_Length = 7;
TempLong = Displacement;
TempLong = TempLong >> 20;
if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Three byte data
B[2] = B[3];
B[3] = B[4];
B[4] = B[5];
Package_Length = 6;
}
TempLong = Displacement;
TempLong = TempLong >> 13;
if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Two byte data
B[2] = B[3];
B[3] = B[4];
Package_Length = 5;
}
TempLong = Displacement;
TempLong = TempLong >> 6;
if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //One byte data
B[2] = B[3];
Package_Length = 4;
}
B[1] += (Package_Length - 4) * 32 + Function_Code;
Make_CRC_Send(Package_Length, B);
}
I don’t think this is a problem with logic and syntax – the code runs well and all the variables are defined properly, along with function calls – but more so that Torque Control will not engage, and as a result, neither will any subsequent Torque Commands. The shaft stays locked.
Another feature I would love to clarify is that Torque control is in principle possible, this is demonstrated by the fact that the motor is able to switch to Torque Control when utilizing the Local Software that the company that provided the motor produced. This is actually what the packages and commands were based off - A C++ software that was modified for Arduino. Additionally position control works excellent in Arduino now and all derived and created functions work great. The same logic was applied to creating Torque Functions
If anyone has previous understanding of RS232 protocol and can help me solve and engage Torque control in Arduino, I would be incredibly grateful. I believe it has to do with the actual value the Torque Switch Command is sending to the “Send Package”, which is both bolded and italicized in the code I sent, and than how that field exists in the Send Package and is ultimately transformed to engage Torque Control.
Additionally as you may notice the Send_Package uses another function called “Make_CRC_Send”, which I include below;
void Make_CRC_Send(unsigned char Plength, unsigned char B[8]) {
unsigned char Error_Check = 0;
char RS232_HardwareShiftRegister;
for (int i = 0; i < Plength - 1; i++) {
OutputBuffer[OutBfTopPointer] = B[i];
OutBfTopPointer++;
Error_Check += B[i];
}
Error_Check = Error_Check | 0x80;
OutputBuffer[OutBfTopPointer] = Error_Check;
OutBfTopPointer++;
while (OutBfBtmPointer != OutBfTopPointer) {
RS232_HardwareShiftRegister = OutputBuffer[OutBfBtmPointer];
//Serial.print("RS232_HardwareShiftRegister: ");
//Serial.println(RS232_HardwareShiftRegister, DEC);
Serial3.write(RS232_HardwareShiftRegister);
OutBfBtmPointer++; // Change to next byte in OutputBuffer to send
}
}
Lastly for further context and help, I will include a folder in OneDrive which has all the parts of the motor used along with the full code that I am using, with only the command to switch to Torque Control. If the code I provided works, what it should achieve is shifting the motor to Torque Control and unlocking the shaft. I can take the rest from there.
Help_-Torque_Command_Motor-_Arduino
If you are interested in the configuration – here is a link to the general application that was followed;
Arduino to DYN AC Servo Drive Serial Communication Example - YouTube
Thank you in advance for all your help and effort.
Marley