Robotic arm torque control

[coder777]

1) line 11 int servo; is unnecessary Simply change line 136 to for (int servo = 0, etc ...), I can not do it because when printing then turns engine [servo] tells me that "servo" is not defined in that loop.

Remove the global variable servo. Line 131: for ( int servo = 0; servo < servo_count; servo++)

The less global variable the better.

regarding to the print of the turns, I have placed it where it was and I have made a loop, but it does not work correctly, since it prints things like this:

Line 163:

Serial.println(turns[servo]);

needs to be

Serial.println(turns[i]);


So do things work like you want it to?

[urko18]

Okey thank you coder777!!

yes, writing this code works:

      Serial.print(F("Turns engine "));
      Serial.print(i+1);
      Serial.print(F(": "));
      Serial.println(turns[i]);
      Serial.println(" ");

this is the result:

Now the servos will do the registered movements.
Turns engine 1: 0
 
Turns engine 2: 0
 
Turns engine 3: 0
 
Turns engine 4: -2

its works correctly yes!!

[doug4]

2) The function, I tried to start it, but without results. I have some doubts:
dest_pos what is it? Where do I define it?
in this line "int curr_pos = dxlGetPosition (SERVO_ID [i])" should not put "servo" instead of "i"?

Yes. I gave you a typo. In some loops you use i. In some places we use servo. I probably cut and pasted from one location and forgot to change it for the other variable name. Just like in the problem that @coder777 fixed for you. There, we were in a loop on i and we used the variable servo.

I am to blame for this confusion. Remember, I changed i to servo in the nested loops so we could more easily keep track or which variable represented which value. (I still stand by that decision.) The confusion is that other loops still use i as an index. For clarity and consistency, it would make sense to change those loop indices to servo rather than i, but I wanted to focus on ways to improve the code structure. I apologize for causing the confusion.

[urko18]

thanks again!

I'm still working with the function but I do not execute it well, if I put it as you put it in the first message, it tells me that dest_pos is undefined. Then I changed that variable name to pos2, also renaming the vector of rest_pos and changing the positionn [servo] [0] also by pos2 (putting it in the loop of line 119, as before). The program does get up and gets complicated, but it does not work correctly, when compiling it it says:

"warning: ISO C++ forbids comparison between pointer and integer [-fpermissive]

else if (pos2 > curr_pos)"

So i don´t know how i can implement the function...for the moment I'll leave it like that.

On the other hand, I am very grateful for all that you have helped me to have a much cleaner and simpler code! Now I would like to ask you about the reason why I wrote in this forum, the torque control of the robot.

It is true that with the code we have left now I see much more clearly where I can and I must control the torque (I guess between the lines (130-142)), but I still do not know exactly what loop or code to use ...

I've been testing with the dxlSetRunningToruqeLimit (...), I mean, a Macro to set running torque limit for the servo. but I do not think it's what I need, I do not need to fix it from the beginning, I need to know if a limit is exceeded or not, and I think it should be better an if or a while, than when I saw that I exceeded the torque limit ( put by me), the robot stopped ...but I still have not quite hit ...


Urko.

[doug4]

It is true that with the code we have left now I see much more clearly where I can and I must control the torque (I guess between the lines (130-142)), but I still do not know exactly what loop or code to use ...

I've been testing with the dxlSetRunningToruqeLimit (...), I mean, a Macro to set running torque limit for the servo. but I do not think it's what I need, I do not need to fix it from the beginning, I need to know if a limit is exceeded or not, and I think it should be better an if or a while, than when I saw that I exceeded the torque limit ( put by me), the robot stopped ...but I still have not quite hit ...

I don't really know what dxlSetRunningTorqueLimit is supposed to do. Since you seemed to have dismissed it anyway, I will too.

What I think you are saying is, while the servos are moving, if a specific torque value is reached, they should stop moving and do something else--maybe return to the rest position or something like that.

I think you need to check the torque every time a servo moves. It looks like dxlSetGoalPosition is the only way to move the servos, and this command is only found in lines 140, 190 and 196. After each of those lines, check the torque on the servo that just moved and if it is too great, handle the error.

I can't tell you how to code up the error handling because I don't really know what you want it to do. You need to design the logic of what you want it to do. You need to decide:

(1) What happens when torque hits max when going to initial positions (in go_to_position)
(2) What happens when torque hits max when moving to registered movements (in loop)
(3) What happens when torque hits max when returning to rest positions (in go_to_position)

It really is a design process, not merely slapping code into what you currently have. For instance, one set of options might be (1) would cause the servos to return to rest positions, but (3) would cause the whole program to simply terminate. [Note: (3) should not merely cause servos to return to rest because that's what's causing the error in the first place.] If this were the case, error handling for go_to_position would have to be outside the function, meaning go_to_position would have to return an error value.

If the error handling for (1) and (3) were the same, you could do the error handling inside the function. Depending on what the error handling entails, go_to_position may still need a return value.

Once you come up with a torque-limit error handling design, you will have a better idea of how to complete your implementation.

Why not start with adding a debug printout of each torque value after each movement? Simplify your test to 1 servo and 10 movements again just to make sure you can read torque values correctly. Once you can, try adding your error-handling design and see where you get.

[urko18]

Hello! My tutor has sent me to do something else (make the program work wirelessly thanks to the Xbee) during these days and I have not been able to make much progress in this!

The torque control would only be for the second point:
(2) What happens when torque hits max when moving to registered movements (in loop).

what I have been testing (with a single engine) has been to set a torque_limit, and put this IF after the WHILE of line 138:

 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;
            Serial.println(torque[servo][movement]);
          while (positionn[servo][movement] != current[servo])
          {
            dxlSetGoalPosition(SERVO_ID[servo], current[servo]);
            if (torque[servo][movement] > torque_limit)
            {
            Serial.println("WARNING torque limit exceed");
            }
            current[servo] += next_pos;
            delayMicroseconds(Speed);

            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]--;
            }
          }
        }
      }
    }

and yes, each time this value is exceeded the program will start the "WARNING torque limit exceed".

But my biggest problem persists, the program registers the torques values when i an doing the sequence of repetitions, that is, at the same time that I define and register positionn I also define and register the torque, (more or less between lines 74-81 of the last program):

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

  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
      torque [j][i] = dxlGetTorque(SERVO_ID[i]); //read and save the actual torque
    }

The problem consists (I think) that as the program have stored the torques at the same time as each position, then when the robot repeats the sequence, the torques do not change, I mean, no matter how hard I press or force the robot, it has saved the torques from before, and these do not change. And what I want is to be able to overcome the torque when I try to stop it myself or some obstacle or something else...

The solution is not what it would be, would I have to calculate the torques during the WHILE? because I tried to do that but I have not been able to get the torque out ...

I do not know if you can help me, but just in case.

many thanks!!

Urko.

[doug4]

First, a side note. You missed one of the comments that I made (I made a bunch, so it's not surprising that you missed one). Line 21 in the last code snippet need to move to before line 16.

Think of it this way. In line 14, you check to see if the current position differs from the target position. If so, move to the current position (line 16) and then adjust the "current" position to get closer to the target position.

Well, the first time into the loop, you won't move the servo. The last time through the loop you change the "current" value after you make your last move, so you never fully move to the target position.


Now, to your problem.

Notice that you are reading torque values at registration. Because your are moving the servos without much resistance while you register positions, torque values are probably pretty low.

In line 17 of the code snippet above, you are comparing the torque values obtained at registration with the torque limit. That is pretty much meaningless. Those values will never change.

Instead, get rid of the torque[servo][movement] array, and just measure torque right after line 16 (dxlGetTorque(SERVO_ID[servo]) and compare that value with torque limit. That way you are comparing real-time data with the limit.

Also, I suggest you strip down to debug mode again and print out the torque values you are reading just so you can see what values you are getting. Leave in the read at registration time and compare it to the values you read after line 16 when you put an obstacle in the way. That should set some light on how things are working.

[urko18]

Thanks doug4 !!

Ok, I've already corrected the comment that I was missing! Thank you!

Today I only had time to do tests with one engine, but it seems to work correctly!

I repeated the same sequence twice, one without any obstacle, and the other putting the hand, and when trying to stop the robot the torque between the two sequences changed, becoming bigger!

Tomorrow I hope that I have time to test with all the engines, and later I will have to set the torque limit more exactly, since for now I have put a value more or less at random.

Thank you very much!

Urko