C++

Forum

 
Robotic arm torque control

Pages: 123
urko18 (59)
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:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
 

//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.
urko18 (59)

Sorry, i forgot to put in the code part the way that i obtain the values of the torque of each motor:



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

This part of the code goes after obtaining the datas of the positions.
coder777 (8439)
So first you could greatly simplify the if on line 33:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
 
      if (positionn1[i] != initial1)
      {
        int next_pos = 1;
        if (positionn1[i] < initial1)
          next_pos = -1;
        while (positionn1[i] != initial1) // EDIT
        {
          before_position_1 = initial1;
          dxlSetGoalPosition(SERVO_ID[0], initial1);
          initial1 += next_pos;
          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);
            }
          }
        }
      }

You should put that code in its own function.

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...
Create a new function that takes the motor id and the destination position. There you let the motor run until the destination position is reached or the torque exceeds the limit. In the first case return true in the latter false. So that you can see after the call whether it is done or not.
Last edited on
urko18 (59)
Many thanks!!

This morning I tried it and it works perfectly! with that reduction I have saved some 100 lines of code, thanks!

Regarding what you tell me to create a function for torque control, I do not quite understand how I should do it and where I should put it.
How do I do that to let the engine work? and then I do an IF structure putting that of the destination position is reached or the torque exceeds the limit?

Could you show me or make an example of the code?

Again, thank you very much,

Urko.
coder777 (8439)
I'm not sure what you're asking.

Regarding what you tell me to create a function for torque control, I do not quite understand how I should do it and where I should put it.
Do you know how to deal with functions?

How do I do that to let the engine work? and then I do an IF structure putting that of the destination position is reached or the torque exceeds the limit?
When I understand you code correctly you do already move the motor to a certain position. Int that case the initial position, but it shouldn't be too hard to put another value.

For the torque maybe something like this?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
 
        while (...)
        {
          before_position = new_pos;
          dxlSetGoalPosition(SERVO_ID[0], new_pos);
          new_pos += next_pos;
          delayMicroseconds(Speed);

          if(dxlGetTorque(...) > torque_limit)
          {
            dxlTorqueOff(...);
            break;
          }

...
}
urko18 (59)
I know how to handle functions, or so I think I do hahah =), what I was asking you (and sorry for my English) is in what part of the code i have to put the function, but i think that I need to get the torque while the arm is moving, so, I have to determine the torque in the while loops, right?


You say to exchange initial for new position? and so put the IF structure of the torque in the same While of each motor of the code?? In which the condition would be that if you exceed the value that I put (torque limit) would stop?
with that break what happens is that it would leave the function, no?
urko18 (59)
In case I do not express myself well, this is my code right now:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
 
#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;
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);
            }
          }
        }
      }
      //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);
            }
          }
        }
      }
urko18 (59)
I can´t put all the program sorry ... this is the continuation:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
 
      //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);
    }
  }
}


So, you say to do a new function to put there the structure of torque control??

Urko.
doug4 (1535)
I have not followed this thread extremely closely, but I have a suggestion for cleaning up the code. Use more arrays (or std::vector if Arduino supports it).

For instance, in line 148 - 151 of your first post, you define 4 variables initial1 through initial4. Then you perform 4 blocks of code in lines 157 - 32 (second post) that are essentially the same, but using initial1 through initial4. If you created an array initial[4] to begin, you could have a simple loop for (int index = 0; index < 4; ++index) { ... initial[i] ... }. That gets rid of almost 3/4 of your code there.

Of course, there are other variables that need to change into arrays, also. For instance, arrays posisionn1 through positionn4 could become 2-dimensional arrays positionn[m][4].

Just about anywhere you define 4 variables in succession, you should replace them with an array. Being able to step through an array and perform common steps on each element is one of the things that makes looping so powerful.

Look at simplifying go_to_position, too. You can change it to take a pos1, a pos2 (horrible variable names, I might add) and an index into SERVO_ID. Call it 4 times, and reduce the clutter in the function. Or, better yet, pass it 2 arrays (a pos1 array and a pos2 array) and have it loop through them. You can get the SERVO_ID index from the loop index.

Any time you find yourself repeating code over and over again, you should consider how to make the code common. That could be using arrays and loops, adding functions, adding templates, or something else. The fewer lines of code written, the clearer it will be, and the easier it will be to read, understand and modify it.
urko18 (59)
Than you doug4!!

I think that is a very good idea, but I don´t know how i can do it. My first doubt is wiht the part of the SERVO_ID, how can I put this in the For loop.

I have done something like this:

1
2
3
4
5
6
7
 
    int positionn[4]
    int next_pos_[4]
    int initial[4]
    int before_position_[4]
    int turns[4]
    int position1
    for (int index = 0; index < 4; ++index) {initial[4], turns[4], positionn[4], next_pos_[4], before_position_[4], position1 }


But i don´t know how i can enter the SERVO_ID (his values are between 0-3 and the structure is SERVO_ID[ ], so if i want to introduce in a fot:

or (int index = 0; index < 4; ++index) { SERVO_ID[ ] }

the program doesn´t run correctly because of the "[ ]"...

Some weeks ago i was trying to do something similar to what yo say to me:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
 
//The next function will repeat the movements of the handcrafted sequence

void repeat_sequence(double initial, double turns, double before_position, double positionn[400], double position, int SERVO_ID[])
{
  if (positionn[i] < initial)
  {
    while (positionn[i] < initial)
    {
      before_position = initial;
      dxlSetGoalPosition(SERVO_ID, initial);
      initial--;
      delayMicroseconds(400);

      if (initial != before_position)
      {
        if (initial == position + MX_MAX_POSITION_VALUE)
        {
          position = initial;
          turns++;
          Serial.print("Turns: ");
          Serial.println(turns);
        }
        else if (initial == position - MX_MAX_POSITION_VALUE)
        {
          position = initial;
          turns--;
          Serial.print("Turns: ");
          Serial.println(turns);
        }
      }
    }
  }
  else if (positionn[i] > initial)
  {
    while (positionn[i] > initial)
    {
      before_position = initial;
      dxlSetGoalPosition(SERVO_ID, initial);
      initial++;
      delayMicroseconds(400);
      if (initial != before_position)
      {
        if (initial == position + MX_MAX_POSITION_VALUE)
        {
          position = initial;
          turns++;
          Serial.print("Turns: ");
          Serial.println(turns);
        }
        else if (initial == position - MX_MAX_POSITION_VALUE)
        {
          position = initial;
          turns--;
          Serial.print("Turns: ");
          Serial.println(turns);
        }
      }
    }
  }
}


And the program run, but the program didn´t run correctly...

So, could you give me more advices or clues please?
thank you again!

Urko.
urko18 (59)
So, in the last program that i have post, before that function, i do this:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
 
  int  positionn;
      int  position;
      int  initial;
      int  before_position;
      int  turns;


      for (i = 0; i < m; i++)
      {
        repeat_sequence(initial1, turns1, before_position_1, positionn1[i], position1, SERVO_ID[0]);
        repeat_sequence(initial2, turns2, before_position_2, positionn2[i], position2, SERVO_ID[1]);
        repeat_sequence(initial3, turns3, before_position_3, positionn3[i], position3, SERVO_ID[2]);
        repeat_sequence(initial4, turns4, before_position_4, positionn4[i], position4, SERVO_ID[3]);
      }
coder777 (8439)
So do you know this documentation?

http://learn.trossenrobotics.com/projects/181-arbotix-1-6-documentation.html

An example of simplification:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
 
#include <ax12.h> //Include ArbotiX DYNAMIXEL library
int kop = 4; //Cuantity of dynamixel
const int SERVO_ID[] = {1, 2, 3, 4}; // const correct / You might want to use just one for testing...
const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID); // Let the compiler calculate the amount of servos, so that you can change it easily

...

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[0]);
  }

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

  delay(1000);

}
Whenever you see code duplication you should create a function with this code. So that you have 4 times function call and not 4 times the same code.

You can actually control the speed with dxlSetGoalSpeed(...). So why a loop like this:
1
2
3
4
5
6
7
 
    while (positionn[i] < initial)
    {
      before_position = initial;
      dxlSetGoalPosition(SERVO_ID, initial);
      initial--;
      delayMicroseconds(400);
...
?

Why do you want to control the torque? There is this function dxlSetRunningToruqeLimit(...)? So why is torque relevant for your processing?

Are you sure that MX_MAX_POSITION_VALUE determines a turn?
urko18 (59)
Thanks again coder777!!!

Yes, I know that documentation, in fact I have used it in some parts of my code. But right now my doubts were more in knowing where I could put the torque control part.

I want to control the torque because I need to make a collaborative robot, that is, in an industry for example, the idea is that the workers can move the robot, in order to do some work, for example to take some boxes.

Torque control goes with the idea of ​​if there is any complication in the repetition of the sequence, the worker is able to grab the robot, and when the robot notice that has to do more torque of the limit (because it has the resistance of the worker), it stop.

So what I need is to know the torque at all times, and if any once the limit is exceeded, I have to stop the execution of the sequence. (sorry because of my english)

REGARDING the minimization of the code (which interests me a lot), I have to admit that I am not a good programmer at all, in fact it is the first time I program in c ++.

I have done the test with the code that you have sent me and it works correctly (I have changed the SERVO_ID [0] for SERVO_ID [i], that I imagine that it would have passed to you),and i also have change all the codes like:
1
2
3
4
 
  Relax(SERVO_ID[0]);
  Relax(SERVO_ID[1]);
  Relax(SERVO_ID[2]);
  Relax(SERVO_ID[3]);


NOW I am focusing on reducing the rest of the code.

I do not know if I can do what I sent you before, or something like that:

1
2
3
4
5
6
7
 
    int positionn[4]
    int next_pos_[4]
    int initial[4]
    int before_position_[4]
    int turns[4]
    int position[4]
    for (int index = 0; index < 4; ++index && int p = 0; p < servo_count; ++p ) {initial[i], turns[i], positionn[i], next_pos_[i], before_position_[i], position[i], SERVO_ID[p] }


???

Or i have to do a function for each int?

Thank you!!


Urko.
urko18 (59)
Continuing answering,

Are you saying that i can change this code (i supose delaymicroseconds) and put instead of that dxlSetGoalSpeed(...)? I don´t unerstand very well sorry.

Are you sure that MX_MAX_POSITION_VALUE determines a turn?


I use that code to calculated the point when the encoder of the robot pass the allowed 4096 positions, in order to know how many turns is doing during the sequence.

Urko.
coder777 (8439)
So what I need is to know the torque at all times, and if any once the limit is exceeded, I have to stop the execution of the sequence.
Well, dxlSetRunningToruqeLimit(...) sounds as if it does exactly that. If so you need to set it only once at the beginning. Plus you do not need to control it yourself.

Are you saying that i can change this code (i supose delaymicroseconds) and put instead of that dxlSetGoalSpeed(...)? I don´t unerstand very well sorry.
If I am right you need to call dxlSetGoalSpeed(...) only the the very beginning. Where you ask for the speed.

I would suggest that you make a simple test program where you check one motor whether the assumption about dxlSetRunningToruqeLimit(...) and dxlSetGoalSpeed(...) and MX_MAX_POSITION_VALUE means one turn (i.e. 360 degrees) is correct. If so it would make your life easier:
1
2
3
4
5
6
 
bool SetPosition(int id, int position)
{
  dxlSetGoalPosition(id, position);
  while(dxlGetMoving(id) != 0) { delay(100); } // Note: delay is system friendly
  return (dxlGetPosition(id) == position); // If returns false, the motor stoppend prematurely, probably because the torque limit exceeded
}
doug4 (1535)
1
2
3
4
5
6
7
 
    int positionn[4]
    int next_pos_[4]
    int initial[4]
    int before_position_[4]
    int turns[4]
    int position[4]
    for (int index = 0; index < 4; ++index && int p = 0; p < servo_count; ++p ) {initial[i], turns[i], positionn[i], next_pos_[i], before_position_[i], position[i], SERVO_ID[p] }


???


I think I know what your question is here. You have a for statement that doesn't do anything. Inside the {...} you simply list a number of values. You probably want to call a function with these values are arguments. Were you trying to call repeat_sequence from a previous post? If so, you do not have the right number of arguments for the function call. And you are passing by value uninitialized values.

I'm a little bit confused about the current state of your code. I have seen thing you have posted and thing that @coder777 has posted, and I really don't know what you have tried and what you have working. I gave you some general suggestions, and @coder777 has given you some more concrete suggestions. Based on the code you had shown, I figured you had more programming experience than you do, so I intentionally was more general to let you figure out on your own what simplification should be done.

Have you successfully made code simplifications yet? I suggest you post the most recent code that you have that works (at least compiles). Then suggestions we make will be based on your current code rather than us guessing what you may or may not have done.

Bye the way, I will try to be more concrete with some of my suggestions so you can see syntactically how the code changes can be made.
urko18 (59)
Hi!

Thanks for answering doug4!

Yes the truth is that I do not have much experience in programming, that's why I do not know how to synthesize / optimize my code.

Between yesterday and today I tried to follow your advices and the advices of coder777, but without result. Right now my code (that compiles and works) is like this,:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
 
/****************************************************************
     Program to control Dynamixel of robotic arm by urko_18
 ****************************************************************/
#include <ax12.h> //Include ArbotiX DYNAMIXEL library
int kop = 4; //Cuantity of dynamixel
const int SERVO_ID[] = {1, 2, 3, 4}; 
const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID); 
double Speed;
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.
  
  for(int i = 0; i < servo_count; ++i)
  {
    Relax(SERVO_ID[i]);
    Serial.print(F("ID: "));
    Serial.println(SERVO_ID[i]);
  }

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

  delay(1000);

}


void loop()
{

  int positionn1[m]; //Matrix of movements
  int positionn2[m];
  int positionn3[m];
  int positionn4[m];
  int i;
  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];
    int turns1 = 0;
    int turns2 = 0;
    int turns3 = 0;
    int turns4 = 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);
            }
          }
        }
      }
      //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);
            }
          }
        }
      }
urko18 (59)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
 
//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);
  
  dxlTorqueOffAll(); 
  
  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);
    }
  }
}
urko18 (59)
yes, my intention was to simplify in a single function or structure the part that takes to ID1 ID2 ID3 ID4, but the truth is that I do not know how to do it ...

in case you want to see what the robot is like (the video is from the summer of 2017), here is a link for the robot:

https://www.youtube.com/watch?v=pJGe6f-Kouw&feature=youtu.be

Thank you!!

Urko.
Last edited on
doug4 (1535)
Here is a rewrite of the loop function to simplify things. I don't believe I changed any logic (other than moving the prints in lines 172-173 and lines 179-180 to a common location after the if-else construct). I made all of your sets of 4 variables into 4-element arrays. Read this and understand how the arrays and the loops work together to make the common code easier to write, read, understand and maintain.

Note that I have not compiled this code, so there may be some syntax errors that I missed. The concepts are sound, but you might have to fix a typo or two.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
 
void loop()
{
  int positionn[servo_count][m]; //Matrix of movements

/* right now no change to the part where you determine repetitions and speed */

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

  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 before_position[servo_count]
    int position[servo_count];
    int turns[servo_count];
    int pos1[servo_count];
    int pos2[servo_count];

    for (int i = 0; i < servo_count; i++) 
    {
      position[i] = position[i][0];
      turns[i] = 0;

      //First, the servos will move to initial position, in order to do the registered movements
      pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
      pos2[i] = positionn[i][0]; //Initial position of the movement (objective)
    }

/* AFTER YOU UNDERSTAND how these simplifications work, rewrite this function */
    go_to_position(pos1[0], pos1[1], pos1[2], pos1[3], pos2[0], pos2[1], pos2[2], pos2[3]); //Function that moves the robot to the initial position

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

    int  initial[servo_count];
    for (int i = 0; i < servo_count; i++) 
    {
      initial[i] = positionn[i][0];
    }

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

            if (initial[servo] != before_position[servo])
            {
              if (initial[servo] == position[sevo] + MX_MAX_POSITION_VALUE)
              {
                position[servo] = initial[servo];
                turns[servo]++;
              }
void loop()
{
              else if (initial[servo] == position[servo] - MX_MAX_POSITION_VALUE)
              {
                position[servo] = initial[servo];
                turns[servo]--;
              }
/* Note:  This might be only approximately correct.  Massage if you need to */
              Serial.print(F("Turns engine "));
              Serial.print(servo);
              Serial.print(F(": "));
              Serial.println(turns1);
            }
          }
        }
      }
    }
    Serial.println(" ");
    delay(1000); //Pause between movements
  }

 /****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 robot position
  }

  //rest position of the robot (the positions of the 4 dynamixel)
  int rest_pos[servo_count] = {3820, 400, 230, 2395};

  go_to_position(pos1[0], pos1[1], pos1[2], pos1[3], rest[0], rest[1], rest[2], rest[3]);//function

  delay(1000);
  
  dxlTorqueOffAll(); 
  
  Serial.println(F("END!"));
}


Notice a few things. In lines 65 and 67 I used variables movement and servo in the nested loops to better clarify what we are looking at (rather than guessing what i and j are and getting them confused).

Next, you should rewrite go_to_position. First of all, the starting positions can be calculated within the function, so there's no need to calculate them outside and pass them in as arguments.

Second, it would be easier to write a function to move a single servo at a time. You could pass in the servo index and the destination position and move one servo. Call the function 4 times to move all of the servos. However, that changes the behavior of you code. Right now, you are interleaving movements of the for servos to move them to where then need to go. If that is the desired behavior, then pass in an array of positions [ go_to_position(int positions[]) ] and reference the positions by the servo indices. You would end up changing lines 102 - 152 into a for loop stepping through the servo indices.

I've given you a lot of suggestions here--more than I intended. Start with the changes I gave you. When you understand them (they should be pretty straight-forward), try to make changes to go_to_position like I suggest. Ask questions if you get stuck. When you get these code changes made, then we can more easily attack some of the issues you were originally asking about.
Last edited on
Pages: 123