Problem to reed torque value from Dynamixel

[urko_18]

Hi again, everybody!
As some of you already know I am improving a program to control a robotic arm with four dynamixel motors (2 MX64 and 2 mx106) in Arduino.


My biggest problem is to control the torque, what I want is to make a collaborative robot, and for that, I need to know the torque that the robot will have everytime, so in the case that the torque is too high, to stop the robot.


My program, today, is the following: (in this case I have set a torque limit of 500 and a serial.print of "Warning torque is exceed", as a test)


#include <ax12.h> //Include ArbotiX DYNAMIXEL library
const int SERVO_ID[] = {1, 2, 3, 4};
const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID); //the compiler calculate the amount of servos
int repetitions, interrupt_state;
double Speed;
int m = 400; //columns
int pos2[] = {3820, 400, 230, 2395}; //rest position of the dynamixel motors
void setup()
{
dxlInit(1000000); //start dynamixel library at 1mbps to communicate with the servos
Serial.begin(9600); //start serial at 9600 for reporting data.


for (int i = 0; i < servo_count; ++i)
{
Relax(SERVO_ID[i]);
Serial.print(F("ID: "));
Serial.println(SERVO_ID[i]);
}


delay(1000);


}


void loop()
{


Serial.println(F("How many times do you want to repeat the sequence?"));


while (Serial.available() == 0) {}
repetitions = Serial.parseInt();
Serial.print(F("repetitions = "));
Serial.println(repetitions);
delay(3000);


Serial.println(F("How fast do you want to repeat the sequence?"));
Serial.println(F(" 1 = Slow 2 = Normal 3 = Fast"));


while (Serial.available() == 0) {}
Speed = Serial.parseInt();


if (Speed <= 0 || Speed >= 4)
{
Serial.println(F("You have entered a wrong value"));
return;
}
if (Speed == 1)
{
Speed = 1000;
}
else if (Speed == 2)
{
Speed = 700;
}
else if (Speed == 3)
{
Speed = 400;
}
Serial.print(F("Speed = "));
Serial.print(Speed, 0);
Serial.println(F(" microseconds"));
delay(3000);


Serial.print(F("Positions vector "));
Serial.print(F(": ["));


int positionn[servo_count][m]; //Matrix of movements


for (int i = 0; i < m; i++) // structure to create columns
{
for (int j = 0; j < servo_count; j++) // structure to create columns
{
positionn[j][i] = dxlGetPosition(SERVO_ID[j]); //read and save the actual position
}
delay(10);


for (int j = 0; j < servo_count; j++) // structure to create columns
{
Serial.print(positionn[j][i]); //Display the vector
Serial.print(F(", "));
}
}
Serial.print(F("]\n"));
delay(5000);


Serial.println(F("The servos will move to the initial position."));
/***The servos will move according to registered movements***/


for (int a = 0; a < repetitions; a++) //Repetition of the process (a = number of sequences)
{
Serial.print(F("SEQUENCE "));
Serial.println(a + 1);


int position[servo_count];
int turns[servo_count];
int pos1[servo_count];
int pos2[servo_count];
int current[servo_count];
int torque_limit = 500;
int torque[servo_count];


for (int i = 0; i < servo_count; i++)
{
current[i] = positionn[i][0];
position[i] = positionn[i][0];
turns[i] = 0;
pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
pos2[i] = positionn[i][0]; //Initial position of the movement (objective)
}
for (int servo = 0; servo < servo_count; ++servo)
{
go_to_position(pos1, pos2, servo); //Function that moves the robot to the initial position
}


Serial.println(F("Now the servos will do the registered movements."));
delay(2000);


for (int movement = 0; movement < m; movement++)
{
for (int servo = 0; servo < servo_count; servo++)
{
if (positionn[servo][movement] != current[servo])
{
int next_pos = 1;
if (positionn[servo][movement] < current[servo])
next_pos = -1;
while (positionn[servo][movement] != current[servo])
{
current[servo] += next_pos;
delayMicroseconds(Speed);

dxlSetGoalPosition(SERVO_ID[servo], current[servo]);
torque[servo] = dxlGetTorque(SERVO_ID[servo]);
Serial.println(torque[servo]);
if (torque[servo] > torque_limit)
{
Serial.println("WARNING torque limit exceed");
}
if (current[servo] == position[servo] + MX_MAX_POSITION_VALUE)
{
position[servo] = current[servo];
turns[servo]++;
}
else if (current[servo] == position[servo] - MX_MAX_POSITION_VALUE)
{
position[servo] = current[servo];
turns[servo]--;
}
}
}
}
}
for (int i = 0; i < servo_count; i++)
{
Serial.print(F("Turns engine "));
Serial.print(i + 1);
Serial.print(F(": "));
Serial.println(turns[i]);
Serial.println(" ");
}
}
delay(3000);


/****REST POSITION****/
Serial.println(F("The robot will move to the resting position."));
int pos1[servo_count];
for (int i = 0; i < servo_count; i++)
{
pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
}
for (int servo = 0; servo < servo_count; ++servo)
{
go_to_position(pos1, pos2, servo); //Function that moves the robot to the initial position
}
delay(1000);
dxlTorqueOffAll();
Serial.println(F("END!"));
}
void go_to_position(int pos1[], int pos2[], int servo)//function
{
while (pos1[servo] != pos2[servo])
{
if (pos2[servo] < pos1[servo])
{
dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
pos1[servo]--;
delayMicroseconds(800);
}
else if (pos2[servo] > pos1[servo])
{
dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
pos1[servo]++;
delayMicroseconds(800);
}
}
}



The problem is that when the robot is moving, when the program prints on the serial monitor the torque that the motors support at all times:

torque[servo] = dxlGetTorque(SERVO_ID[servo]);
Serial.println(torque[servo]);


Almost always gives me a value of -1, and as much as I try to put some obstacle to the robot (with the intention of raising the torque and the message "Warning torque is exceed"), usually does not usually vary the value.


So, I do not know if maybe using dxlGetTorque (ID) is not the correct option, or if what fails is that the ArbotiX-M that is not able to process as much data as position and torque at the same time or i don´t know ...


I hope someone can give me their opinion !!
Many thanks!!

[KurtEck]

The dxlGetTorque simply translatest to:
#define dxlGetTorque(id) (dxlGetRegister(id, AX_PRESENT_LOAD_L, 2))

Which when -1 implies that the dxlGetRegister failed. Why? That is always the hard thing to debug. Maybe packets being output faster than the units can respond? ...

What do I do in cases like this? You can probably already guess from previous posts.... I hook up logic analyzer, and watch the packets to see if they are being sent properly, and if the servo is trying to respond? ... I might also instrument the code with digitalWrite calls around some of these, so we can see when the command is issued and exactly when it returned...

I would also check things like, have all of the return delays of the servos been set to 0 (register 5). Might try checking if adding a small delay between calls helps...

Would check to see if your servos are setup for Protocol 1 or protocol 2? I am assuming protocol 1, if they work at all, but if you are protocol 2, your servo register tables are different... But than again so are the packets...

But if you go to Protocol 2, then you might be able to use things like SyncRead for reading the Present Current or the like, but everything is different... Also not sure if you have support for this with Arbotix board... Do with OpenCM...

[urko_18]

Thank you for answering KurtEck,

First of all, yes my servos are protocol 1, and yes I have an arobtix-m board to conect the servos with the computer, I´am working with arduino.


I would also check things like, have all of the return delays of the servos been set to 0 (register 5). Might try checking if adding a small delay between calls helps...

I tried to do that, but the program have done the same...


I hook up logic analyzer, and watch the packets to see if they are being sent properly, and if the servo is trying to respond?
I do not understand what you mean, sorry. At the moment I tried to do the following test:
I have done this basic program:

The program calculates the torque of the motors everytime, that is, when I move the robotic arm the different torques change. With which I deduce that the dxlGetTorque (id) block works correctly.

I would say, seeing how the general program responds, that having to be calculating the position and also the torque of each motor, which is not able to process everything at once ... maybe I should put some delay after wanting to print the torque? maybe Is that part of the code too fast?

I know that my SRAM memory of the microcontroller is almost full (as you know I´am triying to save the positions in a SD) when I say to the program to save 400 movements, but the thing is that the program also works baddly (also put -1 when is learning the torque) if I only say to save 10 movements. So, I didn´t think that it can be a memory problem no?

Urko.

[urko_18]

This is the basic program of the torque that I have mentiones in the previous post:



#include <ax12.h>
int SERVO_ID[] = {1, 2, 3, 4}; //números ID de los motores que se están utilizando
int regData5, regData6, regData7, regData8;
int u = 400; //columns
int v = 1; //rows
int w, z, j=0;






void setup() {


dxlInit(1000000); //start dynamixel library at 1mbps to communicate with the servos
Serial.begin(9600); //start serial at 9600 for reporting data.


Relax(SERVO_ID[0]);
Relax(SERVO_ID[1]);
Relax(SERVO_ID[2]);
Relax(SERVO_ID[3]);


Serial.print("ID: ");
Serial.println(SERVO_ID[0]);
Serial.print ("ID: ");
Serial.println(SERVO_ID[1]);
Serial.print("ID: ");
Serial.println(SERVO_ID[2]);
Serial.print("ID: ");
Serial.println(SERVO_ID[3]);
delay(1000);


}


void loop() {

int torque1[v][u];
int torque2[v][u];
int torque3[v][u];
int torque4[v][u];

while (j<1){

for (w = 0; w < v; w++)
{
Serial.print(" torque vector");
Serial.print(w+1);
Serial.print(": [");


for (z = 0; z < u; z++)
{


regData5 = dxlGetTorque(SERVO_ID[0]);
regData6 = dxlGetTorque(SERVO_ID[1]);
regData7 = dxlGetTorque(SERVO_ID[2]);
regData8 = dxlGetTorque(SERVO_ID[3]);


torque1[w][z] = regData5;
torque2[w][z] = regData6;
torque3[w][z] = regData7;
torque4[w][z] = regData8;


delay(20);


Serial.print(torque1[w][z]); //print the torque vector
Serial.print(", ");
Serial.print(torque2[w][z]);
Serial.print(", ");
Serial.print(torque3[w][z]);
Serial.print(", ");
Serial.print(torque4[w][z]);
Serial.print("; ");
}
}
delay(5000);
Serial.println("END");
j++;
}
}

[KurtEck]

Hi @urko_18,

Sorry I missed your last post on this a couple months ago. I see you asked the same question recently up on the Robotis forum:
http://en.robotis.com/service/forum_view.php?bbs_no=2406870&page=1&save_sca=&sca=&save_stx=&stx=&sfl=

In case you missed it, I posted up there that I think I know at least some of the problem. That is, with code like:

dxlSetGoalPosition(SERVO_ID[servo], current[servo]);
torque[servo] = dxlGetTorque(SERVO_ID[servo]);

That by default whenever you do a logic set operation, like the dxlSetGoalPosition, by default the servo is going
to respond with a response packet, which by default the library does not wait until the response happens. This
very possibly interfere if you try to send another packet right after the set message. This could be another set message or could be a query message, in
either case you could end up with two things trying to write to the DXL buss at the same time...

There are a variety of ways to solve this. Probably the simplest is to add the call to wait until the response packet is received.

dxlSetGoalPosition(SERVO_ID[servo], current[servo]);
dxlReadPacket();
torque[servo] = dxlGetTorque(SERVO_ID[servo]);

Another way is to tell the servos to not send a response packet. This is controlled by a servo register:
http://emanual.robotis.com/docs/en/dxl/mx/mx-64/#status-return-level-16

Another way is to use a command that does not generate a response. Example if you are going to set the position of 4 servos,
instead of doing 4 set goal positions, you can instead do a sync write with the goal position to all 4 servos...

[urko_18]

Thanks KurtEck!!

Yes, yesterday I saw the message in the other forum, but it was really messed up and I could not answer, I am already in my last days of the project.


I'll try the options you tell me, and I'll tell you the results!


Thank you very much again!!


Urko.

[urko_18]

I have tried to put the dxlReadPacket (current [servo]); and it actually reads the torques, but the problem is that it moves very slowly, it takes too long to make the movements because is waiting to read the torque so...

[urko_18]

The thing is that if I set so that Status Packet will not be returned for all Instructions, I probably can´t know the positions no?

[KurtEck]

It is hard to give you any complete answer without knowing the current code and the how all of the servos are configured. It might help to see hopefully a small self contained program that illustrates the problem...

That is, again if you set the servo return level (http://emanual.robotis.com/docs/en/dxl/mx/mx-64/#status-return-level-16)to 1, default is 2, then you will still send you back packets if you do query like ask for the current position or the current torque. It will not send one back when do things like set positions.... Which may not matter here as you are not looking at the return data anyway (which has error data).

As for code speed... There are lots of factors that may influence this. One of the biggest with something like this, is how long does it take to get a return packet, (One like mentioned here, but also ones from your actual query calls). This can be impacted a lot by the return delay time (http://emanual.robotis.com/docs/en/dxl/mx/mx-64/#return-delay-time-5) set for each of the servos. I typically always set this value to 0, default is 254 (508us delay).

But another major influence is the actual code. That is if your code still looks something like:


for (int movement = 0; movement < m; movement++)
{
for (int servo = 0; servo < servo_count; servo++)
{
if (positionn[servo][movement] != current[servo])
{
int next_pos = 1;
if (positionn[servo][movement] < current[servo])
next_pos = -1;
while (positionn[servo][movement] != current[servo])
{
current[servo] += next_pos;
delayMicroseconds(Speed);

dxlSetGoalPosition(SERVO_ID[servo], current[servo]);
torque[servo] = dxlGetTorque(SERVO_ID[servo]);
Serial.println(torque[servo]);
if (torque[servo] > torque_limit)
{
Serial.println("WARNING torque limit exceed");
}
if (current[servo] == position[servo] + MX_MAX_POSITION_VALUE)
{
position[servo] = current[servo];
turns[servo]++;
}
else if (current[servo] == position[servo] - MX_MAX_POSITION_VALUE)
{
position[servo] = current[servo];
turns[servo]--;
}
}
}
}
}

Obvious things beyond the message times mentioned above are if you are still doing Serial.print... in the loops,
that will slow things down...

In these loops you are only moving one servo at a time, that is you move SERVO_ID[0] to it's new position, then move
SERVO_ID[1], to its position. And for each of these moves you have delays for your speed... Might be better if you
organized the code to update all of the servos once and then do the delay...

And if you can organize the code that way, then instead of doing individual set goal positions, you can use one
SYNC write call to update all of the servos with one message. That removes many of the messages. I am assuming that you
are running this servos Protocol 1 (as the library is setup for Protocol 1...), but if they are setup for Protocol 2, than
you could potentially use the SYNC Read to again read in all of the servos with one message... But probably not a valid
option for this case.

But again a lot depends on the actual code...

[urko_18]

Hello KurtEck! thanks for answering me again !!


On July 5 I will present my project on the robotic arm and I will not have more time to continue with the project. These days I've been quite busy working with all the documentation and presentation, and that's why I have not been able to put into practice the advice you have given me these last days.


The next course (in September), I imagine that another person will continue with this same project, with which, I will no longer be working on the project. Yes, I have already left this other links to continue working on this torque control ...


Thank you very much to you and to all those who have helped me!


Urko.