C++

Forum

 
Robotic arm torque control

Pages: 123
urko18 (59)
oh really thank you! It helps me a lot to continue with the project!

I have been reading and understanding your code this morning, I have corrected a few typical syntax errors like sevo instead of servo and things like that ... but now I have some doubts:

1) why are there two void loop? (the beginning and the line 88)

2) When passing the vector position to two dimensions, I do not know how to match this:
 
 
position[i] = position[i][0];


The program says that "invalid types 'int[int]' for array subscript", and i don´t know how i can change position [i], because i understand that [i] are the 1,2,3,4 motors no? so i don´t know how i can equal to 2d array.

3) Also the program gives errors (for example: error: 'rest' was not declared in this scope go_to_position(pos1[0], pos1[1], pos1[2], pos1[3], rest[0], rest[1], rest[2], rest[3]);//function) about the funciton go_to_position, what is it that I'm supposed to do no? i will try to do it now.

But,i can eliminate this FOR no? (lines 114-117)
1
2
3
4
 
  for (int i = 0; i < servo_count; i++) 
  {
     pos1[i] = dxlGetPosition(SERVO_ID[i]); //actual robot position
  }

Because i have definited the actual robot position in the line 49 no?

4) error: array must be initialized with a brace-enclosed initializer

int next_pos[servo] = 1;

This is the other erro that the program gives to me, and i don´t know how i can solucionate. I understand that you want to give the value 1 to nex position of each engine, but since now all the engines are done at the same time, I understand that the program believes that nex position is another array, no? How could I solve it?

Thank you,

Urko.

urko18 (59)
i have done the function of the rest position:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
 
    void go_to_position(pos1[0], pos1[1], pos1[2], pos1[3], rest_pos[0], rest_pos[1], rest_pos[2], rest_pos[3]);//function
    {
      while (pos_1[servo] != rest_pos[servo_count])
      {
        if (rest_pos[servo_count] < pos_1[servo])
        {
          dxlSetGoalPosition(SERVO_ID[servo], pos_1[servo]);
          pos_1--;
          delayMicroseconds(800);
        }
        else if (rest_pos[servo_count] > pos_1[servo])
        {
          dxlSetGoalPosition(SERVO_ID[servo], pos_1[servo]);
          pos_1++;
          delayMicroseconds(800);
        }
      }
    }


I don´t know if is ok, because the program gives me some aditional errors like:


1) error: variable or field 'go_to_position' declared void

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

^

2) error: 'servo' was not declared in this scope

while (pos_1[servo] != rest_pos[servo_count])

^
I supose that i have to put "servo" like global variable no?

3) error: lvalue required as decrement operand

pos_1--;

^

4) error: lvalue required as increment operand

pos_1++;

^
urko18 (59)
i THINK that i have correct the errors 3) and4)
3) error: lvalue required as decrement operand

pos_1--;

^

4) error: lvalue required as increment operand

pos_1++;


I forgot to put "[servo]" in the code like that:

pos_1[servo]++;


doug4 (1534)
I'm not sure what all you have already fixed. Here are answers to the ones you may not have resolved yet.

1) why are there two void loop? (the beginning and the line 88)


The one in line 88 is a cut and paste error. I don't really know where it came from, but it should not be there.

position[i] = position[i][0];


The right hand side should be positionn[i][0]. The positionn array is 2 dimensional.

Note that naming 2 arrays position and positionn is highly confusing for this very reason. I strongly suggest you rename one of them to make it easier to read and comprehend.

Edit: I realize that your code had position1, position2, etc. which I changed to position[4]. Your original names were not as confusing as the array I changed them to. I was trying to reuse your names as much as possible, and the renaming ended up causing confusion.

1) error: variable or field 'go_to_position' declared void

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

^


You cannot declare/define a function like that. A function definition must give types for its arguments. This line is how you would call the function.

The function signature (based on the code you are showing) should be:

void go_to_position(int pos1[], int rest_pos[], int servo)

Notice that your signature contained values whereas my function signature declared variables of type int[] and int.

2) error: 'servo' was not declared in this scope

while (pos_1[servo] != rest_pos[servo_count])

^


If you change the function signature, servo is now being passed into the function.

There are more efficiencies that can be realized in this function call. Why don't you get it working as much as you can (or you get stuck) and post your latest code?
Last edited on
urko18 (59)
Thank you for answering again!

The right hand side should be positionn[i][0]. The positionn array is 2 dimensional.


Yes, but I do not understand how I can fix that error. How am I going to match two different dimensions?

Notice that your signature contained values whereas my function signature declared variables of type int[] and int.


Oh is true, i don´t know how i can work with this variables inside a function...


If you change the function signature, servo is now being passed into the function.

Yes, know i was triying to solve those errors that i saw to you, and them my idea was to sumarize the function, but know I have to leave the job until tomorrow.

urko18 (59)
The code is like that:

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
 
#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
int 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.

  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()
{

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

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

  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)
    }

    go_to_position(pos1[], rest_pos[], servo); //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 (int 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[servo] + MX_MAX_POSITION_VALUE)
              {
                position[servo] = initial[servo];
                turns[servo]++;
              }
              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(turns[servo]);
            }
          }
        }
      }
      Serial.println(" ");
      delay(1000); //Pause between movements
    }

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

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


    delay(1000);

    dxlTorqueOffAll();

    Serial.println(F("END!"));
  }
}
  void go_to_position(int pos1[], int rest_pos[], int servo)//function
  {
    while (pos1[servo] != rest_pos[servo_count])
    {
      if (rest_pos[servo_count] < pos1[servo])
      {
        dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
        pos1[servo]--;
        delayMicroseconds(800);
      }
      else if (rest_pos[servo_count] > pos1[servo])
      {
        dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
        pos1[servo]++;
        delayMicroseconds(800);
      }
    }
  }
ISR(PCINT1_vect) {
  interrupt_state = (PINB & 0x02) >> 1;
  Serial.println("EMERGENCY BUTTON!");
}



And the errors that have are those:



line 115: error: invalid types 'int[int]' for array subscript

position[i] = position[i][0];

^

line 123: error: expected primary-expression before ']' token

go_to_position(pos1[], rest_pos[], servo); //Function that moves the robot to the initial position

^

line 123: error: 'rest_pos' was not declared in this scope

go_to_position(pos1[], rest_pos[], servo); //Function that moves the robot to the initial position

^

line 123: error: expected primary-expression before ']' token

go_to_position(pos1[], rest_pos[], servo); //Function that moves the robot to the initial position

^

line 123: error: 'servo' was not declared in this scope

go_to_position(pos1[], rest_pos[], servo); //Function that moves the robot to the initial position

^

line 140: error: array must be initialized with a brace-enclosed initializer

int next_pos[servo] = 1;

^

exit status 1
invalid types 'int[int]' for array subscript

doug4 (1534)
line 115: error: invalid types 'int[int]' for array subscript

position[i] = position[i][0];

^


Change righthand side to positionn[i][0]

line 123: error: expected primary-expression before ']' token

go_to_position(pos1[], rest_pos[], servo); //Function that moves the robot to the initial position

^


The arrays are named pos1 and rest_pos. The brackets mean nothing here. The are only meaningful when declaring an array or function argument or when you are dereferencing an index in the array. Get rid of both "[]".

line 123: error: 'rest_pos' was not declared in this scope

go_to_position(pos1[], rest_pos[], servo); //Function that moves the robot to the initial position

^


The variable rest_post is not declared until line 176 (in the code you posted--off by 3 lines from the error messages.) To be consistent with your original post, you actually want to pass the array pos2 here.

line 123: error: expected primary-expression before ']' token

go_to_position(pos1[], rest_pos[], servo); //Function that moves the robot to the initial position

^


Again, remove the "[]".


line 123: error: 'servo' was not declared in this scope

go_to_position(pos1[], rest_pos[], servo); //Function that moves the robot to the initial position

^


OK. You are trying to mix in parts of my suggestions but not all of them. The original call to go_to_position took values for all 4 positions and all 4 fall-back positions. I suggested that you pass in a single set of positions and the servo # (so you could get the servoID) and call the function for each servo. This means you need to put the function call into a loop (for (int servo = 0; servo < servo_count; ++servo)), and then you can pass the loop index into go_to_position as the value servo. You can't change the way the function is written without changing the way the function is called.

line 140: error: array must be initialized with a brace-enclosed initializer

int next_pos[servo] = 1;

^


I never actually declare the array next_pos[servo_count]; in the code that you copied. Declare right after line 125 (right after the declaration of the initial array).
urko18 (59)
Okey!

I have already corrected the errors I had, thanks!

The program compiles and uploads to the arbotix-M, but it does not work correctly.

Now, the program code 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
 
#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); 
int repetitions;
double Speed;
int m = 450; //columns
#include <avr/interrupt.h>
#define PULSADOR_EMERGENCIA (PINB&0x02)
int interrupt_state;
//rest position of the robot (the positions of the 4 dynamixel)
int rest_pos[servo_count] = {3820, 400, 230, 2395};

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()
{

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

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

  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] = positionn[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)
    }
    for (int servo = 0; servo < servo_count; ++servo)
    {
      go_to_position(pos1, rest_pos, servo); //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];
    int next_pos[servo_count];
    for (int i = 0; i < servo_count; i++)
    {
      initial[i] = positionn[i][0];
    }

    for (int movement = 0; movement < m; movement++)
    {
      for (int servo = 0; servo < servo_count; servo++)
      {
        if (positionn[servo][movement] != initial[servo])
        {
          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[servo] + MX_MAX_POSITION_VALUE)
              {
                position[servo] = initial[servo];
                turns[servo]++;
              }
              else if (initial[servo] == position[servo] - MX_MAX_POSITION_VALUE)
              {
                position[servo] = initial[servo];
                turns[servo]--;
              }
              
              Serial.print(F("Turns engine "));
              Serial.print(servo);
              Serial.print(F(": "));
              Serial.println(turns[servo]);
            }
          }
        }
      }
      Serial.println(" ");
      delay(1000); //Pause between movements
    }

    /****REST POSITION****/
    Serial.println(F("The robot will move to the resting position."));
    delay(1000);
    dxlTorqueOffAll();
    Serial.println(F("END!"));
  }
}
void go_to_position(int pos1[], int rest_pos[], int servo)//function
{
  while (pos1[servo] != rest_pos[servo_count])
  {
    if (rest_pos[servo_count] < pos1[servo])
    {
      dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
      pos1[servo]--;
      delayMicroseconds(800);
    }
    else if (rest_pos[servo_count] > pos1[servo])
    {
      dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
      pos1[servo]++;
      delayMicroseconds(800);
    }
  }
}
ISR(PCINT1_vect) {
  interrupt_state = (PINB & 0x02) >> 1;
  Serial.println("EMERGENCY BUTTON!");
}
urko18 (59)
It starts well, it asks you the two questions (repetitions and speed), and it starts taking the data of the positions of the engines correctly. That is, i move the robot and the program takes the positions of each engine correctly.

The problem start when the robot has to return to the initial position to star repiting the sequence (from line 98 onwards), i don´t know why the robot moves to other position (not initial position), and stays stop, and even more, the alarm led of the third engine is turned on, and i don´t know why. I only have tried it twice , because I do not want it to damage.
Them, the robot stays stopped, with the led turn on, and meanwhile appears in the output "Turns engine 3: 0"

Here is the output of the program:


ID: 1
ID: 2
ID: 3
ID: 4
How many times do you want to repeat the sequence?
repetitions = 1
How fast do you want to repeat the sequence?
 1 = Slow     2  = Normal     3 = Fast
Speed = 1000 microseconds
Positions vector : [3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2397, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802, 379, 221, 2398, 3802,....]

The servos will move to the initial position.
SEQUENCE 1
Now the servos will do the registered movements.
 
 
 
 
Turns engine 3: 0
 
Turns engine 3: 0
 
 
 
 
 
 
 
 
 
Turns engine 3: 0
 
Turns engine 3: 0
 
 
 
 
 
 
 
 
Turns engine 3: 0
 
 
Turns engine 3: 0


I tried to find out the problem/error comparing this code with the previous one, but I can´t find it, I know that it is difficult for you without trying the program to deduce something but ... do you see any error?

Thank you,

Urko.
coder777 (8439)
That is, i move the robot and the program takes the positions of each engine correctly.
No it doesn't. As you can see the values are all the same. You need a dialog with the user in order to get the correct values. I.e. You ask the user to go to a first position and when the user confirms that it is the correct position you can get the value. The same for second, third, etc...

The computere is by far faster than the motor.

I would think that
1
2
3
4
 
            before_position[servo] = initial[servo];
            dxlSetGoalPosition(SERVO_ID[servo], initial[servo]);
            initial[servo] += next_pos[servo];
            delayMicroseconds(Speed);
is not a good way to send the position. You may send a position before the last position was processed.
As I told you before you should send the position once and wait until it is reached.

I strongly recommend that you do this with one motor only until you know how it reacts. Then the second and so on.
urko18 (59)
the thing is that i have only posted in the last message the first values, when i didn´t start to move the robot yet, but i assure you that them the values change.





As I told you before you should send the position once and wait until it is reached.


But the program, with the optimization that you have done runs and works correctly, why you suggest to change it?



doug4 (1534)
I agree with everything the @coder777 suggests. I strongly agree that you start with a single servo. That's easy enough by changing line 3 in your code:

3
4
 
//const int SERVO_ID[] = {1, 2, 3, 4}; 
const int SERVO_ID[] = {1};


The number of servos, and thus all of your loop counters and array indices, are based on this line of code. If you only include 1 servo ID, you simplify your system. Adding the other servos back in is simply adding the IDs back to the array.

I would also suggest setting movement to a smaller number (like 10) until you get the rest of the logic working.

After trying @coder777's suggestions, if you still are having problems, add more print statements in the loop. Like, right after line 144, print out servo, movement, positionn[servo][movement], initial[servo] and next_pos[servo]. Then after line 149, print out before_position[servo], initial[servo] and turns[servo]. These 2 sets of printout will be only for debugging, so there is no need to make them pretty--just print out the information so you know what the values are (make sure you put spaces between the numbers so you can read them).

Run the program and look at the values you print out. Use the value to figure out what the code is doing, and compare it to what you want it to do. That should tell you what needs to change. When you see something is not working as it should, describe the problem and the expected behavior, and we can help you debug it some more.



p.s. I am looking back at the code you have now. It looks much better than it did. Realize that my suggestions for most of the changes was renaming your variables to make them arrays where appropriate. Now that the code is more cleaned up, I see places for more efficiencies. However, let's put them on hold until we get your code working.

urko18 (59)
I just tried it with a single motor and 50 movements, and the program does not work properly either. I have put to repeat the sequence twice and to go at a slow speed. The robot have took correctly the positions:

How many times do you want to repeat the sequence?
repetitions = 2
How fast do you want to repeat the sequence?
 1 = Slow     2  = Normal     3 = Fast
Speed = 1000 microseconds
Positions vector : [-983, -998, -1015, -1034, -1049, -1068, -1084, -1101, -1117, -1133, -1149, -1165, -1181, -1196, -1210, -1224, -1239, -1251, -1265, -1277, -1292, -1304, -1318, -1330, -1343, -1354, -1366, -1378, -1390, -1403, -1415, -1426, -1438, -1450, -1461, -1474, -1486, -1499, -1511, -1525, -1537, -1550, -1562, -1576, -1590, -1603, -1619, -1635, -1650, -1665, ]
The servos will move to the initial position.


but them, the robot repeats the sequence to fast (not slowly) and them stays in the same position a long time, and meanwhile in the program output appear this:

SEQUENCE 1
Now the servos will do the registered movements.
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
Turns engine 0: 0
 
....and more and more....


And when some time is past, the robot does the repetition of the second sequence (again fastly)


after, he has again put: Turns engine 0: 0

and finally (some long time after) it had print :

The robot will move to the resting position.
END!

And the program had ended.

Know i have to go home, but tomorrow i will also do test (I will print what you say doug4)...and i will say to you the results..

Thank you both!!!

Urko.
urko18 (59)
Today I have done some tests putting the prints that doug4 told me.
I have done them with one engine (ID4) and with 20 movements.

To begin with, what I have clear is that my function go_to_position does not work (I do not know why), since at the end of the program the robot does not return to the resting position, it stays in the last position of the sequence. [The last time I put my code in the post, I forgot to put the function at the end, but it's set between lines 178-179]

Second, the program regist all movements correctly, I have checked it with other programs that what they do is return the position, and

Third, the robot repeat the sequence, but very fast, I have the impression that with the program as it is now, I do not know why but it does not pay attention to delaymicroseconds funciton... so I do not know if I would have to fix dxlSetGoalSpeed ​​(...) just like Coder777 told me.

Could this be done in my program by putting at the beginning (after the speed question) like this? do not?

dxlSetGoalSpeed ​​(Speed)


Finally, i will saw you part of the output of the program:

ID: 4
How many times do you want to repeat the sequence?
repetitions = 1
How fast do you want to repeat the sequence?
 1 = Slow     2  = Normal     3 = Fast
Speed = 1000 microseconds
Positions vector : [847, 847, 848, 847, 848, 848, 848, 848, 848, 850, 860, 879, 903, 932, 959, 984, 1006, 1020, 1028, 1029, ]
The servos will move to the initial position.
SEQUENCE 1
Now the servos will do the registered movements.
 
 
linea 144
servo: 0
movement: 2
positionn: 848
next_pos: 1
intial: 847
linea 149
before_position: 847
initial: 848
turns: 0
Turns engine 0: 0
 
linea 144
servo: 0
movement: 3
positionn: 847
next_pos: -1
intial: 848
linea 149
before_position: 848
initial: 847
turns: 0
Turns engine 0: 0
 
linea 144
servo: 0
movement: 4
positionn: 848
next_pos: 1
intial: 847
linea 149
before_position: 847
initial: 848
turns: 0
Turns engine 0: 0
 
 
 
 
 
linea 144
servo: 0
movement: 9
positionn: 850
next_pos: 1
intial: 848
linea 149
before_position: 848
initial: 849
turns: 0
Turns engine 0: 0
linea 149
before_position: 849
initial: 850
turns: 0
Turns engine 0: 0
 
linea 144
servo: 0
movement: 10
positionn: 860
next_pos: 1
intial: 850
linea 149
before_position: 850
initial: 851
turns: 0
Turns engine 0: 0
linea 149
before_position: 851
initial: 852
turns: 0
Turns engine 0: 0
linea 149
before_position: 852
initial: 853
turns: 0
Turns engine 0: 0
linea 149
before_position: 853
initial: 854
turns: 0
Turns engine 0: 0
linea 149
before_position: 854
initial: 855
turns: 0
Turns engine 0: 0
linea 149
before_position: 855
initial: 856
turns: 0
Turns engine 0: 0
linea 149
before_position: 856
initial: 857
turns: 0
Turns engine 0: 0
linea 149
before_position: 857
initial: 858
turns: 0
Turns engine 0: 0
linea 149
before_position: 858
initial: 859
turns: 0
Turns engine 0: 0
linea 149
before_position: 859
initial: 860
turns: 0
Turns engine 0: 0


As I said before, the robot makes the sequence (or part of the sequence) very fast, but then, while in the output puts the prints that, the robot is moving very slowly, until the end comes to the position final, and I do not know why it is ...
Last edited on
urko18 (59)
What I understand, to begin with, is that I would have to take those prints (those from lines 165-168 out of the loop, but I'm not sure either.) For example, when the robot has to go from position 848 to 850, the line 149 is printed twice, if the robot has to go from 848 to 853, the prints of the line 149 will print 5 times...and I do not know exactly why.

I can not understand why the program makes a fast movement first, in which it covers almost the entire sequence, and then, it goes much slower.

Urko.
doug4 (1534)
For example, when the robot has to go from position 848 to 850, the line 149 is printed twice, if the robot has to go from 848 to 853, the prints of the line 149 will print 5 times...and I do not know exactly why.

Welcome to the world of programming. This is debugging. You need to compare the results with the code to figure out what's happening. That is whey I had you add the debug prints--to get more results to look at.

For the question of why are we looping 5 times to go from 848 to 853, look at these lines:
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
 
          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[servo] + MX_MAX_POSITION_VALUE)
              {
                position[servo] = initial[servo];
                turns[servo]++;
              }
              else if (initial[servo] == position[servo] - MX_MAX_POSITION_VALUE)
              {
                position[servo] = initial[servo];
                turns[servo]--;
              }
              
              Serial.print(F("Turns engine "));
              Serial.print(servo);
              Serial.print(F(": "));
              Serial.println(turns[servo]);
            }
          }


The next_pos array value will always contain either 1 or -1 (lines 142 - 144).

Then, you loop until positionn equals initial. Inside the loop, you:
- Assign before_position to initial (to be used to see if initial changed)
- move the servo to the value that initial currently has
- add next_pos to initial (remember, you are only adding 1 or -1, so a change of 5 means 5 times through the loop)
- check to see if initial changed (no longer == before_position). If so
- - Perform a check against MAX and MIN values (I don't think this check is correct, so turns never get modified)
- - print out the results
- loop again (unless initial now equals positionn.

Because you only increment or decrement the initial variable by 1, you need n loops to move n positions. How do you want this servo to move? Can you just move from 848 to 853 directly? Are there limits to how far you can move at once?

Moving the final prints out of the loop will print them only once after all of the loop iterations are complete.

Finally, what are you trying to do with lines 154 - 163? I don't think you are succeeding.
urko18 (59)
Hi!

This morning I continued doing tests (first with one engine and then I have already added all) and changing some things and finally I got it to work !! Thank you very much both!!!

this is how the code is 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
 
#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); 
int repetitions;
double Speed;
int m = 400; //columns
#include <avr/interrupt.h>
#define PULSADOR_EMERGENCIA (PINB&0x02)
int interrupt_state;
int servo;
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()
{

  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 = 100;
  }
  Serial.print(F("Speed = "));
  Serial.print(Speed, 0);
  Serial.println(F(" microseconds"));
  delay(3000);

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

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

  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] = positionn[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)
    }
    for (int servo = 0; servo < servo_count; ++servo)
    {
      go_to_position(pos1, pos2, servo); //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];
    int next_pos[servo_count];
    for (int i = 0; i < servo_count; i++)
    {
      initial[i] = positionn[i][0];
    }

    for (int movement = 0; movement < m; movement++)
    {
      for ( servo = 0; servo < servo_count; servo++)
      {
        if (positionn[servo][movement] != initial[servo])
        {
          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[servo] + MX_MAX_POSITION_VALUE)
              {
                position[servo] = initial[servo];
                turns[servo]++;
              }
              else if (initial[servo] == position[servo] - MX_MAX_POSITION_VALUE)
              {
                position[servo] = initial[servo];
                turns[servo]--;
              }
            }
          }
        }
      }
    }
   // Serial.print(F("Turns engine "));
    //Serial.print(servo);
    //Serial.print(F(": "));
    //Serial.println(turns[servo]);
    //Serial.println(" ");
  }
  delay(3000);
  /****REST POSITION****/
  Serial.println(F("The robot will move to the resting position."));
  //rest position of the robot (the positions of the 4 dynamixel)
  int pos1[servo_count];
  int pos2[] = {3820, 400, 230, 2395};
  for (int i = 0; i < servo_count; i++)
  {
    pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
  }
  for (int servo = 0; servo < servo_count; ++servo)
  {
    go_to_position(pos1, pos2, servo); //Function that moves the robot to the initial position
  }
  delay(1000);
  dxlTorqueOffAll();
  Serial.println(F("END!"));
}
void go_to_position(int pos1[], int pos2[], int servo)//function
{
  while (pos1[servo] != pos2[servo])
  {
    if (pos2[servo] < pos1[servo])
    {
      dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
      pos1[servo]--;
      delayMicroseconds(800);
    }
    else if (pos2[servo] > pos1[servo])
    {
      dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
      pos1[servo]++;
      delayMicroseconds(800);
    }
  }
}
ISR(PCINT1_vect) {
  interrupt_state = (PINB & 0x02) >> 1;
  Serial.println("EMERGENCY BUTTON!");
}

Last edited on
urko18 (59)
The only doubt is that I do not know where to put the print of the line (167-171), since that was what made everything so slow, yes i know that i an so rookie in this.

Finally, what are you trying to do with lines 154 - 163? I don't think you are succeeding.


In those lines I calculate the number of laps that each engine gives, and of course I always put 0 laps because in the tests to small movements, and did not give time to give a full turn, but now with 400 movements, it tells me correctly the amount of turns.


p.s. I am looking back at the code you have now. It looks much better than it did. Realize that my suggestions for most of the changes was renaming your variables to make them arrays where appropriate. Now that the code is more cleaned up, I see places for more efficiencies.


If you have some new ideas I'll be happy to hear them!!

Again, thank you,

Urko.
Last edited on
doug4 (1534)
The only doubt is that I do not know where to put the print of the line (167-171), since that was what made everything so slow, yes i know that i an so rookie in this.

Where they were you were printing them for every movement for every servo. If you simply want to print them at the end of each movement, move them up 1 line (after line 165). You will need to wrap them in a for (server = 0; etc...) loop to be able to print info for all 4 servos. If you just want to print after every repetition, leave the lines where you have them, but wrap them in a for loop.

In those lines I calculate the number of laps that each engine gives, and of course I always put 0 laps because in the tests to small movements, and did not give time to give a full turn, but now with 400 movements, it tells me correctly the amount of turns.

Then I guess it is sort of doing what you want it to do. If you turn the servo 2 complete times within a single movement, you won't catch it. It might be easier to just subtract the destination position from the starting position and divide by MX_MAX_POSITION_VALUE at the end to determine how many turns were made.

If you have some new ideas I'll be happy to hear them!!

First, you never use the variable "kop". I believe it has been replaced by servo_count.

Also, line 11 int servo; is unnecessary. Simply change line 136 to for (int servo = 0; etc...)

Move line 127 up to after line 105, and move line 131 to after line 112 (keep position and initial together).

The array variable before_position[] is only used inside a loop indexing through the servos. The value is not used outside the loop, so the value for one servo does not need to be distinguished from another servo. Thus, the array can be replaced by a single, non-array before_position variable. Go ahead and declare it where it is assigned (line 145).

int before_position = initial[servo];

Of course, get rid of the index in line 150, also.

The next_pos array is used like the before_postion array, so it can be made a single variable also.



Since you changed go_to_position to change 1 motor at a time, there is no reason to pass arrays of values to the function any more. Now, just the info for the servo being moved needs to be passed. Change the function to the following:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
 
void go_to_position(int dest_pos, int servo)//function
{
  int curr_pos = dxlGetPosition(SERVO_ID[i]);

  while (curr_pos != dest_post)
  {
    if (dest_post < curr_pos)
    {
      curr_pos--;
      dxlSetGoalPosition(SERVO_ID[servo], curr_pos);
      delayMicroseconds(800);
    }
    else if (dest_pos > curr_pos)
    {
      curr_pos++;
      dxlSetGoalPosition(SERVO_ID[servo], curr_pos);
      delayMicroseconds(800);
    }
  }
}


Note the following:

1. Only the destination position and the servo number are required as inputs.

2. The starting position can be calculated here so you don't need to calculate it in the loop() function before this function is called.

3. The curr_pos value is incremented/decremented before setting the goal position so that the servo will actually move during the first iteration. Before you were determining the curr_pos, moving to the curr_pos, and then changing the curr_pos. The end result is that you would always end up 1 spot away from the desired target position.

With these changes, you can get rid of lines 107, 108, 115, 116 and 117 (declaring and populating pos1 and pos2 arrays) and change line 121 to:

go_to_position(positionn[servo][0], servo); //Function that moves the robot to the initial position

Likewise, you can get rid of lines 177 and 179-182 (declaring and populating pos1 array) and change line 185 to

go_to_position(pos2[servo], servo); //Function that moves the robot to the rest position



I would highly recommend moving line 178 to the beginning of the file (like after line 4) so that it is easy to find and maintain. Also, I would recommend renaming the array to rest_pos[servo_count], a more meaningful name. This, of course needs to be reflected in line 185.



I would recommend renaming the array initial[] (line 127) which is initialized to the starting position but then changes throughout the movements. I thinks "current" would be a better name.



I don't think you actually need the before_position variable at all. You are assigning it to the value from initial[servo] in line 145 and then modifying initial[servo] by adding next_pos. The next_pos value is never 0, so before_position will never be the same as initial[servo] at line 150, so you don't need before_position.
urko18 (59)
Thank you very much for answering doug4, sorry but I've been involved with another project of a racing car that I have to do ...

Right now, after making some of the changes you proposed, the code 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
 
#include <ax12.h> //Include ArbotiX DYNAMIXEL library
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
int repetitions, servo, interrupt_state;
double Speed;
int m = 400; //columns
#include <avr/interrupt.h>
#define PULSADOR_EMERGENCIA (PINB&0x02)
int pos2[] = {3820, 400, 230, 2395}; //rest position of the dynamixel motors
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()
{

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

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

  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 position[servo_count];
    int turns[servo_count];
    int pos1[servo_count];
    int pos2[servo_count];
    int current[servo_count];
    
    for (int i = 0; i < servo_count; i++)
    {
      current[i] = positionn[i][0];
    }
    for (int i = 0; i < servo_count; i++)
    {
      position[i] = positionn[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)
    }
    for (int servo = 0; servo < servo_count; ++servo)
    {
      go_to_position(pos1, pos2, servo); //Function that moves the robot to the initial position
    }

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

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

              if (current[servo] == position[servo] + MX_MAX_POSITION_VALUE)
              {
                position[servo] = current[servo];
                turns[servo]++;
              }
              else if (current[servo] == position[servo] - MX_MAX_POSITION_VALUE)
              {
                position[servo] = current[servo];
                turns[servo]--;
              }
          }
        }
      }
    }
    for (int i = 0; i < servo_count; i++)
    {
      Serial.print(F("Turns engine "));
      Serial.print(servo);
      Serial.print(F(": "));
      Serial.println(turns[servo]);
      Serial.println(" ");
    }
  }
  delay(3000);
  
  /****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 servo position
  }
  for (int servo = 0; servo < servo_count; ++servo)
  {
    go_to_position(pos1, pos2, servo); //Function that moves the robot to the initial position
  }
  delay(1000);
  dxlTorqueOffAll();
  Serial.println(F("END!"));
}
void go_to_position(int pos1[], int pos2[], int servo)//function
{
  while (pos1[servo] != pos2[servo])
  {
    if (pos2[servo] < pos1[servo])
    {
      dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
      pos1[servo]--;
      delayMicroseconds(800);
    }
    else if (pos2[servo] > pos1[servo])
    {
      dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
      pos1[servo]++;
      delayMicroseconds(800);
    }
  }
}
ISR(PCINT1_vect) {
  interrupt_state = (PINB & 0x02) >> 1;
  Serial.println("EMERGENCY BUTTON!");
}


The changes that I have left to make are:

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

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

The rest is changed, I think.

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

SEQUENCE 1
Now the servos will do the registered movements.
Turns engine 4: 3827
 
Turns engine 4: 3827
 
Turns engine 4: 3827
 
Turns engine 4: 3827

And i don´t know how i can correct that.

Lastly, regarding to the way of calculate the number of turns of each engine, I've been testing codes like this,(inside a loop for the 4 engines):
 
 
turns(servo)== ((dxlSetGoalPosition(SERVO_ID[servo], current[servo])-current(servo)) / MX_MAX_POSITION_VALUE)


But without results, what you said was something like that or I'm lost?

Again, thank you very much!

Urko.
Pages: 123