 # Robotic arm torque control

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:

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

regData1 = dxlGetPosition(SERVO_ID); //read and save the actual position
regData2 = dxlGetPosition(SERVO_ID);
regData3 = dxlGetPosition(SERVO_ID);
regData4 = dxlGetPosition(SERVO_ID);

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;
int position2 = positionn2;
int position3 = positionn3;
int position4 = positionn4;

//initial is the position whrn the sequence starts

int  initial1 = positionn1;
int  initial2 = positionn2;
int  initial3 = positionn3;
int  initial4 = positionn4;

//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, 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, 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.

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

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

regData1 = dxlGetPosition(SERVO_ID); //read and save the actual position
regData2 = dxlGetPosition(SERVO_ID);
regData3 = dxlGetPosition(SERVO_ID);
regData4 = dxlGetPosition(SERVO_ID);

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);
regData6 = dxlGetTorque(SERVO_ID);
regData7 = dxlGetTorque(SERVO_ID);
regData8 = dxlGetTorque(SERVO_ID);

//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;
int position2 = positionn2;
int position3 = positionn3;
int position4 = positionn4;

//initial is the position whrn the sequence starts

int  initial1 = positionn1;
int  initial2 = positionn2;
int  initial3 = positionn3;
int  initial4 = positionn4;

//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, 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, 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
``````
``````    regData1 = dxlGetPosition(SERVO_ID); //read and save the actual position
regData2 = dxlGetPosition(SERVO_ID);
regData3 = dxlGetPosition(SERVO_ID);
regData4 = dxlGetPosition(SERVO_ID);

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.

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)

``````/********************************************************************************
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);
Relax(SERVO_ID);
Relax(SERVO_ID);
Relax(SERVO_ID);

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

Serial.print(F("ID: "));
Serial.println(SERVO_ID);
Serial.print(F("ID: "));
Serial.println(SERVO_ID);
Serial.print(F("ID: "));
Serial.println(SERVO_ID);
Serial.print(F("ID: "));
Serial.println(SERVO_ID);
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); //read and save the actual position
positionn2[i] = dxlGetPosition(SERVO_ID);
positionn3[i]  = dxlGetPosition(SERVO_ID);
positionn4[i] = dxlGetPosition(SERVO_ID);

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;
int position2 = positionn2;
int position3 = positionn3;
int position4 = positionn4;

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

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

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;
int  initial2 = positionn2;
int  initial3 = positionn3;
int  initial4 = positionn4;

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, 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);
}
}
}
}
...
``````

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

``````     //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, 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, 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, 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); //actual robot position
int pos1_2 = dxlGetPosition(SERVO_ID);
int pos1_3 = dxlGetPosition(SERVO_ID);
int pos1_4 = dxlGetPosition(SERVO_ID);
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);
Relax(SERVO_ID);
Relax(SERVO_ID);
Relax(SERVO_ID);

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, pos1_1);
pos1_1--;
delayMicroseconds(800);
}
else if (pos2_1 > pos1_1)
{
dxlSetGoalPosition(SERVO_ID, pos1_1);
pos1_1++;
delayMicroseconds(800);
}
//ID2
if (pos2_2 < pos1_2)
{
dxlSetGoalPosition(SERVO_ID, pos1_2);
pos1_2--;
delayMicroseconds(800);
}
else if (pos2_2 > pos1_2)
{
dxlSetGoalPosition(SERVO_ID, pos1_2);
pos1_2++;
delayMicroseconds(800);
}
//ID3
if (pos2_3 < pos1_3)
{
dxlSetGoalPosition(SERVO_ID, pos1_3);
pos1_3--;
delayMicroseconds(800);
}
else if (pos2_3 > pos1_3)
{
dxlSetGoalPosition(SERVO_ID, pos1_3);
pos1_3++;
delayMicroseconds(800);
}
//ID4
if (pos2_4 < pos1_4)
{
dxlSetGoalPosition(SERVO_ID, pos1_4);
pos1_4--;
delayMicroseconds(800);
}
else if (pos2_4 > pos1_4)
{
dxlSetGoalPosition(SERVO_ID, pos1_4);
pos1_4++;
delayMicroseconds(800);
}
}
}
``````
``````int i, repetitions;
``````

There is NO excuse for one letter global variable names.

``````double Speed;
``````

Speed is populated by a call to Serial.parseInt(). Why is Speed a double?

``````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.

I don´t understand you, but I've already removed the global variable and I have put it in the void setup loop

There is NO excuse for one letter global variable names.

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.

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

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.

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?

``````        regData5 = dxlGetTorque(SERVO_ID);
regData6 = dxlGetTorque(SERVO_ID);
regData7 = dxlGetTorque(SERVO_ID);
regData8 = dxlGetTorque(SERVO_ID);
``````

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.

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.

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 ); // TorqueOn (SERVO_ID ); // TorqueOn (SERVO_ID );

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

Urko.

urko_18: ```         regData5 = dxlGetTorque(SERVO_ID);         regData6 = dxlGetTorque(SERVO_ID);         regData7 = dxlGetTorque(SERVO_ID);         regData8 = dxlGetTorque(SERVO_ID); ```

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

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.