Go Down

Topic: Robotic arm torque control (Read 945 times) previous topic - next topic

urko_18

Hello everyone, I am an engineering student and I am doing my final career year practices.

In these practices, I have to implement the control of a robotic arm by arduino.

I have already done most of the control, what I have done has been that first, you have to move the robotic arm as you want and meanwhile the program keeps the positions of the motors. Then, once the positions are saved, the robot repeats the sequence of movements that you have made.

What I want now is to control the torque, that is, if at any moment one of the engines passes a certain torque level, the robot have to stop and must go to a position determined by me.

I already have the torque values of each motor at all times, where I need help is to know in what part of the code and how I can put the structure IF / WHILE or whatever to implement torque control in the program.

This is my program code summarized:

Code: [Select]

//first, i obtained the positions of each motor (4)

    regData1 = dxlGetPosition(SERVO_ID[0]); //read and save the actual position
    regData2 = dxlGetPosition(SERVO_ID[1]);
    regData3 = dxlGetPosition(SERVO_ID[2]);
    regData4 = dxlGetPosition(SERVO_ID[3]);

    positionn1[i] = regData1; //Creation movements vector
    positionn2[i] = regData2;
    positionn3[i] = regData3;
    positionn4[i] = regData4;

//once i have done the movements, them i do some comparison to move the robot

    int before_position_1, before_position_2, before_position_3, before_position_4;
    int position1 = positionn1[0];
    int position2 = positionn2[0];
    int position3 = positionn3[0];
    int position4 = positionn4[0];

//initial is the position whrn the sequence starts

    int  initial1 = positionn1[0];
    int  initial2 = positionn2[0];
    int  initial3 = positionn3[0];
    int  initial4 = positionn4[0];


//THE START OF COMPARISONS FOR THE REPETITION OF THE SEQUENCE

//ID1
      if (positionn1[i] < initial1)
      {
        while (positionn1[i] < initial1)
        {
          before_position_1 = initial1;
          dxlSetGoalPosition(SERVO_ID[0], initial1);
          initial1--;
          delayMicroseconds(Speed);

          if (initial1 != before_position_1)
          {
            if (initial1 == position1 + MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1++;
              Serial.print("Turns engine 1: ");
              Serial.println(turns1);
            }
            else if (initial1 == position1 - MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1--;
              Serial.print("Turns engine 1: ");
              Serial.println(turns1);
            }
          }
        }
      }
      else if (positionn1[i] > initial1)
      {
        while (positionn1[i] > initial1)
        {
          before_position_1 = initial1;
          dxlSetGoalPosition(SERVO_ID[0], initial1);
          initial1++;
          delayMicroseconds(Speed);
          if (initial1 != before_position_1)
          {
            if (initial1 == position1 + MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1++;
              Serial.print("Turns engine 1: ");
              Serial.println(turns1);
            }
            else if (initial1 == position1 - MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1--;
              Serial.print("Turns engine 1: ");
              Serial.println(turns1);
            }
          }
        }
      }

//THEM, AFTER THIS CODE, SAME CODE COULD BE FOR THE OTHER 3 ENGINES...(changing the varibals of each one)

//ID2
//ID3
//ID4

    Serial.println(" ");
    delay(1000); //Pause between movements
  }
  delay(7000);

go_to_position(pos1_1, pos1_2, pos1_3, pos1_4, pos2_1, pos2_2, pos2_3, pos2_4);//function to return de robot to a resting position



As you can see, it is the same code for the 4 motors changing the varibals of each one, and my question is where I could put the concidition that if a certain torque were exceeded, the execution of the program would stop.

I don´t know what structure i have to use, and neither if i have to put in all the code of each motor or if i can put this condition only one time at the top or at the end...

Many thanks!

Urko.

urko_18

Sorry, I didn´t put in the part of the code the way that I obtain the torque values of the motors:

Code: [Select]


//first, i obtained the positions of each motor (4)

    regData1 = dxlGetPosition(SERVO_ID[0]); //read and save the actual position
    regData2 = dxlGetPosition(SERVO_ID[1]);
    regData3 = dxlGetPosition(SERVO_ID[2]);
    regData4 = dxlGetPosition(SERVO_ID[3]);

    positionn1[i] = regData1; //Creation movements vector
    positionn2[i] = regData2;
    positionn3[i] = regData3;
    positionn4[i] = regData4;

//I ALSO OBTAIN THE TORQUE VALUES OF EACH MOTOR

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

//once i have done the movements, them i do some comparison to move the robot

    int before_position_1, before_position_2, before_position_3, before_position_4;
    int position1 = positionn1[0];
    int position2 = positionn2[0];
    int position3 = positionn3[0];
    int position4 = positionn4[0];

//initial is the position whrn the sequence starts

    int  initial1 = positionn1[0];
    int  initial2 = positionn2[0];
    int  initial3 = positionn3[0];
    int  initial4 = positionn4[0];


//THE START OF COMPARISONS FOR THE REPETITION OF THE SEQUENCE

//ID1
      if (positionn1[i] < initial1)
      {
        while (positionn1[i] < initial1)
        {
          before_position_1 = initial1;
          dxlSetGoalPosition(SERVO_ID[0], initial1);
          initial1--;
          delayMicroseconds(Speed);

          if (initial1 != before_position_1)
          {
            if (initial1 == position1 + MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1++;
              Serial.print("Turns engine 1: ");
              Serial.println(turns1);
            }
            else if (initial1 == position1 - MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1--;
              Serial.print("Turns engine 1: ");
              Serial.println(turns1);
            }
          }
        }
      }
      else if (positionn1[i] > initial1)
      {
        while (positionn1[i] > initial1)
        {
          before_position_1 = initial1;
          dxlSetGoalPosition(SERVO_ID[0], initial1);
          initial1++;
          delayMicroseconds(Speed);
          if (initial1 != before_position_1)
          {
            if (initial1 == position1 + MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1++;
              Serial.print("Turns engine 1: ");
              Serial.println(turns1);
            }
            else if (initial1 == position1 - MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1--;
              Serial.print("Turns engine 1: ");
              Serial.println(turns1);
            }
          }
        }
      }

//THEM, AFTER THIS CODE, SAME CODE COULD BE FOR THE OTHER 3 ENGINES...(changing the varibals of each one)

//ID2
//ID3
//ID4

    Serial.println(" ");
    delay(1000); //Pause between movements
  }
  delay(7000);

go_to_position(pos1_1, pos1_2, pos1_3, pos1_4, pos2_1, pos2_2, pos2_3, pos2_4);//function to return de robot to a resting position


PaulS

Code: [Select]
    regData1 = dxlGetPosition(SERVO_ID[0]); //read and save the actual position
    regData2 = dxlGetPosition(SERVO_ID[1]);
    regData3 = dxlGetPosition(SERVO_ID[2]);
    regData4 = dxlGetPosition(SERVO_ID[3]);

    positionn1[i] = regData1; //Creation movements vector
    positionn2[i] = regData2;
    positionn3[i] = regData3;
    positionn4[i] = regData4;

Why do you need regData1, regData2, etc.? Why not just directly store the value returned by dxlGetPosition() in positionnN?

Why isn't positionn a 2D array?

You have WAY too much duplication of data going on. You need to post ALL of your code.

Obviously, you need to get the torque while the arm is moving. That means that you have to determine the torque in the while loops, NOT once before the while loop starts.
The art of getting good answers lies in asking good questions.

urko_18

Thank you for answering!

During this morning I have been reducing the code, and besides what you have told me about regdata1, ... my code now is like this: (Do not listen to the part of the emergency button that is still undeveloped)

Code: [Select]

/********************************************************************************
     Program to control Dynamixel of robotic arm by urko_18
 ********************************************************************************/
#include <ax12.h> //Include ArbotiX DYNAMIXEL library
int kop = 4; //Cuantity of dynamixel
int SERVO_ID[] = {1, 2, 3, 4}; // ID numbers
int turns1 = 0;
int turns2 = 0;
int turns3 = 0;
int turns4 = 0;
int i, repetitions;
double Speed;
/*****(r = 1; m = 420) (r = 2; m = 210) (r = 3; m = 140) (r = 4; m = 105)*****/ // maximun "m" for differents "r"
int m = 450; //columns
#include <avr/interrupt.h>
#define PULSADOR_EMERGENCIA (PINB&0x02)
int interrupt_state;

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

  PORTB |= 0x1E;
  PCICR |= (1 << PCIE1);
  PCMSK1 |= 0x1E;
  interrupt_state = 0;
  interrupts();

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

}


void loop()
{

  int positionn1[m]; //Matrix of movements
  int positionn2[m];
  int positionn3[m];
  int positionn4[m];

  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(": ["));

  for (i = 0; i < m; i++) // structure to create columns
  {
    positionn1[i] = dxlGetPosition(SERVO_ID[0]); //read and save the actual position
    positionn2[i] = dxlGetPosition(SERVO_ID[1]);
    positionn3[i]  = dxlGetPosition(SERVO_ID[2]);
    positionn4[i] = dxlGetPosition(SERVO_ID[3]);
    
    delay(10);

    Serial.print(positionn1[i]); //Display the vector
    Serial.print(F(", "));
    Serial.print(positionn2[i]);
    Serial.print(F(", "));
    Serial.print(positionn3[i]);
    Serial.print(F(", "));
    Serial.print(positionn4[i]);
    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 before_position_1, before_position_2, before_position_3, before_position_4;
    int position1 = positionn1[0];
    int position2 = positionn2[0];
    int position3 = positionn3[0];
    int position4 = positionn4[0];

    //First, the servos will move to initial position, in order to do the registered movements

    int pos1_1 = dxlGetPosition(SERVO_ID[0]); //Actual servo position
    int pos1_2 = dxlGetPosition(SERVO_ID[1]);
    int pos1_3 = dxlGetPosition(SERVO_ID[2]);
    int pos1_4 = dxlGetPosition(SERVO_ID[3]);
    int pos2_1 = positionn1[0]; //Initial position of the movement (objective)
    int pos2_2 = positionn2[0];
    int pos2_3 = positionn3[0];
    int pos2_4 = positionn4[0];

    go_to_position(pos1_1, pos1_2, pos1_3, pos1_4, pos2_1, pos2_2, pos2_3, pos2_4); //Function that moves the robot to the initial position

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

    //Now, they will move through the registered positions

    int  initial1 = positionn1[0];
    int  initial2 = positionn2[0];
    int  initial3 = positionn3[0];
    int  initial4 = positionn4[0];

    for (i = 0; i < m; i++)
    {

      //ID1
      if (positionn1[i] != initial1)
      {
        int next_pos_1 = 1;
        if (positionn1[i] < initial1)
          next_pos_1 = -1;
        while (positionn1[i] != initial1)
        {
          before_position_1 = initial1;
          dxlSetGoalPosition(SERVO_ID[0], initial1);
          initial1 += next_pos_1;
          delayMicroseconds(Speed);

          if (initial1 != before_position_1)
          {
            if (initial1 == position1 + MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1++;
              Serial.print(F("Turns engine 1: "));
              Serial.println(turns1);
            }
            else if (initial1 == position1 - MX_MAX_POSITION_VALUE)
            {
              position1 = initial1;
              turns1--;
              Serial.print(F("Turns engine 1: "));
              Serial.println(turns1);
            }
          }
        }
      }
...



urko_18

he does not let me put all the program sorry ... this is the continuation:

Code: [Select]

     //ID2
      if (positionn2[i] != initial2)
      {
        int next_pos_2 = 1;
        if (positionn2[i] < initial2)
          next_pos_2 = -1;
        while (positionn2[i] != initial2)
        {
          before_position_2 = initial2;
          dxlSetGoalPosition(SERVO_ID[1], initial2);
          initial2 += next_pos_2;
          delayMicroseconds(Speed);

          if (initial2 != before_position_2)
          {
            if (initial2 == position2 + MX_MAX_POSITION_VALUE)
            {
              position2 = initial2;
              turns2++;
              Serial.print(F("Turns engine 2: "));
              Serial.println(turns2);
            }
            else if (initial2 == position2 - MX_MAX_POSITION_VALUE)
            {
              position2 = initial2;
              turns2--;
              Serial.print(F("Turns engine 2: "));
              Serial.println(turns2);
            }
          }
        }
      }
      //ID3
      if (positionn3[i] != initial3)
      {
        int next_pos_3 = 1;
        if (positionn3[i] < initial3)
          next_pos_3 = -1;
        while (positionn3[i] != initial3)
        {
          before_position_3 = initial3;
          dxlSetGoalPosition(SERVO_ID[2], initial3);
          initial3 += next_pos_3;
          delayMicroseconds(Speed);

          if (initial3 != before_position_3)
          {
            if (initial3 == position3 + MX_MAX_POSITION_VALUE)
            {
              position3 = initial3;
              turns3++;
              Serial.print(F("Turns engine 3: "));
              Serial.println(turns3);
            }
            else if (initial3 == position3 - MX_MAX_POSITION_VALUE)
            {
              position3 = initial3;
              turns3--;
              Serial.print(F("Turns engine 3: "));
              Serial.println(turns3);
            }
          }
        }
      }
      //ID4
      if (positionn4[i] != initial4)
      {
        int next_pos_4 = 1;
        if (positionn4[i] < initial4)
          next_pos_4 = -1;
        while (positionn4[i] != initial4)
        {
          before_position_4 = initial4;
          dxlSetGoalPosition(SERVO_ID[3], initial4);
          initial4 += next_pos_4;
          delayMicroseconds(Speed);

          if (initial4 != before_position_4)
          {
            if (initial4 == position4 + MX_MAX_POSITION_VALUE)
            {
              position4 = initial4;
              turns4++;
              Serial.print(F("Turns engine 4: "));
              Serial.println(turns4);
            }
            else if (initial4 == position4 - MX_MAX_POSITION_VALUE)
            {
              position4 = initial4;
              turns4--;
              Serial.print(F("Turns engine 4: "));
              Serial.println(turns4);
            }
          }
        }
      }
    }
    Serial.println(" ");
    delay(1000); //Pause between movements
  }
  delay(5000);


  /****REST POSITION****/
  Serial.println(F("The robot will move to the resting position."));

  int pos1_1 = dxlGetPosition(SERVO_ID[0]); //actual robot position
  int pos1_2 = dxlGetPosition(SERVO_ID[1]);
  int pos1_3 = dxlGetPosition(SERVO_ID[2]);
  int pos1_4 = dxlGetPosition(SERVO_ID[3]);
  int pos2_1 = 3820; //rest position of the robot (the positions of the 4 dynamixel)
  int pos2_2 = 400;
  int pos2_3 = 230;
  int pos2_4 = 2395;

  go_to_position(pos1_1, pos1_2, pos1_3, pos1_4, pos2_1, pos2_2, pos2_3, pos2_4);//function

  delay(1000);

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

  Serial.println(F("END!"));
}
ISR(PCINT1_vect) {
  interrupt_state = (PINB & 0x02) >> 1;
  Serial.println("EMERGENCY BUTTON!");
}
//The following function will move the robot to the initial position and to the resting position
void go_to_position(int pos1_1, int pos1_2, int pos1_3, int pos1_4, int pos2_1, int pos2_2, int pos2_3, int pos2_4)
{
  while (pos1_1 != pos2_1 || pos1_2 != pos2_2 || pos1_3 != pos2_3 || pos1_4 != pos2_4)
  {
    //ID1
    if (pos2_1 < pos1_1)
    {
      dxlSetGoalPosition(SERVO_ID[0], pos1_1);
      pos1_1--;
      delayMicroseconds(800);
    }
    else if (pos2_1 > pos1_1)
    {
      dxlSetGoalPosition(SERVO_ID[0], pos1_1);
      pos1_1++;
      delayMicroseconds(800);
    }
    //ID2
    if (pos2_2 < pos1_2)
    {
      dxlSetGoalPosition(SERVO_ID[1], pos1_2);
      pos1_2--;
      delayMicroseconds(800);
    }
    else if (pos2_2 > pos1_2)
    {
      dxlSetGoalPosition(SERVO_ID[1], pos1_2);
      pos1_2++;
      delayMicroseconds(800);
    }
    //ID3
    if (pos2_3 < pos1_3)
    {
      dxlSetGoalPosition(SERVO_ID[2], pos1_3);
      pos1_3--;
      delayMicroseconds(800);
    }
    else if (pos2_3 > pos1_3)
    {
      dxlSetGoalPosition(SERVO_ID[2], pos1_3);
      pos1_3++;
      delayMicroseconds(800);
    }
    //ID4
    if (pos2_4 < pos1_4)
    {
      dxlSetGoalPosition(SERVO_ID[3], pos1_4);
      pos1_4--;
      delayMicroseconds(800);
    }
    else if (pos2_4 > pos1_4)
    {
      dxlSetGoalPosition(SERVO_ID[3], pos1_4);
      pos1_4++;
      delayMicroseconds(800);
    }
  }
}

PaulS

Code: [Select]
int i, repetitions;
There is NO excuse for one letter global variable names.

Code: [Select]
double Speed;
Speed is populated by a call to Serial.parseInt(). Why is Speed a double?

Code: [Select]
void loop()
{

  int positionn1[m]; //Matrix of movements
  int positionn2[m];
  int positionn3[m];
  int positionn4[m];

On which Arduino is this code running? 1800 ints won't fit in the SRAM of most Arduinos.

Have you seen a real robot arm move? Multiple joints are moving at a time, so that the end effector follows the desired path. Moving one joint at time, from the current position to the target position is not how a real robot moves. I KNOW that this has been pointed out before.
The art of getting good answers lies in asking good questions.

urko_18

I don´t understand you, but I've already removed the global variable and I have put it in the void setup loop
Quote
There is NO excuse for one letter global variable names.
Quote
Speed is populated by a call to Serial.parseInt(). Why is Speed a double?
Because its has more than one value, it depends of the number that you put (1,2 or 3), and if i put Speed like Int, the program didn´t run correctly.

I am using the arbotiX-M, so i have space in the SRAM (is almost full) and the porgram run correctly.


Of course I have seen a robotic arm, but in this project the aim is that the arm copies the movements that have previously been made.

My doubt remains in how can I do and where can I do the control of the torque.

Urko.


slipstick

From what little of your code you have posted it looks like you are only reading the torque of each motor after all the movements have been completed and the motors have stopped moving. How does that tell you anything useful?

Steve


PaulS

Quote
My doubt remains in how can I do and where can I do the control of the torque.
The FIRST thing you need to decide is WHEN you plan to control the torque. Once you know that, where is trivial.

Of course, you are ASSuming that you can get from the joint angle condition where over-torque happens to some resting position without additional over-torque happening. I've got some news for you, and it isn't good news.
The art of getting good answers lies in asking good questions.

urko_18

Quote
From what little of your code you have posted it looks like you are only reading the torque of each motor after all the movements have been completed and the motors have stopped moving. How does that tell you anything useful?
Code: [Select]

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


This allows to my to have the values of the torque all the time, so i can and i have to use when it is moving.

Quote
The FIRST thing you need to decide is WHEN you plan to control the torque. Once you know that, where is trivial.
My intention is to control the torque when the robot is repeating the sequence that I have marked.

Quote
Of course, you are ASSuming that you can get from the joint angle condition where over-torque happens to some resting position without additional over-torque happening. I've got some news for you, and it isn't good news.
Yes indeed, I had already thought about it, so perhaps my idea is that when the torque limit is exceeded, by using of this command:


// TorqueOn (SERVO_ID
  • );

// TorqueOn (SERVO_ID [1]);
// TorqueOn (SERVO_ID [2]);
// TorqueOn (SERVO_ID [3]);

The motors will stop, and once they are stopped, I will send the robot to the resting position by using the SetPosition command.

Urko.

slipstick

Code: [Select]

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


This allows to my to have the values of the torque all the time, so i can and i have to use when it is moving.
Except those lines aren't in your latest "complete" code because you seem to have taken all reference to anything to do with "torque" out of it. This is far too confusing for me.

Steve

PaulS

Quote
My intention is to control the torque when the robot is repeating the sequence that I have marked.
The C++ language doesn't have a when statement, so looking for one in your code would be a waste of time.

I suggest that you mean while the robot is repeating the sequence. I suggest, too, that that is far too broad a scope. It is NOT "while repeating the sequence" that matters. It is "while moving a joint from one position to another". And THAT is easy enough to find in your code. It happens in 4 places, which is three too many.

There should be a function, moveJoint() that takes the joint (number/instance), the from value, and the to value, and moves the joint from here to there, monitoring the torque each step of the way. That function should return a value, defining whether the torque was exceeded, or not. If it was, the program needs to break out of the for loop that is iterating the moves AND out of the for loop that is iterating the number of times to repeat the sequence.
The art of getting good answers lies in asking good questions.

Go Up