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