C++

Forum

 
 
How save servos position in a SD card

Pages: 12
urko18 (59)
Hi to all of you!

I'm doing the programming of a robotic arm (maybe some of you knows me because it's not the first time I ask around here), and this time my question is this:

I have this program made to move the robot. The program, what it does, very briefly, is to save some positions of the four motors that form the robot (I manually move the robot) after saving the positions, the robot repeats the movements that I have done previously:

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
 
#include <ax12.h> //Include ArbotiX DYNAMIXEL library
const int SERVO_ID[] = {1, 2, 3, 4};
const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID);
int repetitions, interrupt_state;
double Speed;
int m = 430; //columns
int pos2[] = {1750, 400, 1500, 1330}; //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]);
  }
  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];
      position[i] = positionn[i][0];
      turns[i] = 0;
      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 (int 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(i + 1);
      Serial.print(F(": "));
      Serial.println(turns[i]);
      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);
    }
  }
}


My problem is that I have to save the movements in an SD card, since the SRAM memory of the microcontroller that I use only saves me 100 movements of each motor, and it is not enough.

So, what I would like to know is how and in which part of the code I should pass this vector to the SD, in order to save much more positions:

positionn[j][i] = dxlGetPosition(SERVO_ID[j]);



I have done this simple program that allows to pass the movements to the SD card, but i don´t know how i can integrate this simple program in the main program...

Can anyone help me?

Thanks!

Urko.
urko18 (59)
the simple program that i have mentiones in the previos post is 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
 
#include <ax12.h> //Incluir la librería ArbotiX DYNAMIXEL
int kop = 4; //Cantidad de motores utilizados
int SERVO_ID[] = {1, 2, 3, 4}; //números ID de los motores que se están utilizando
int regData5, regData6, regData7, regData8;
int u = 200; //columnas -> cantidad de posiciones guardadas por cada movimiento
int v = 1; //filas -> cantidad de movimientos guardados
int w, z, j=0;

#include <SPI.h>
#include <SD.h>
File myFile;

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]); //los motores se pueden mover
  Relax(SERVO_ID[1]);
  Relax(SERVO_ID[2]);
  Relax(SERVO_ID[3]);

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

  Serial.begin(9600);
  Serial.print("Iniciando SD ...");
  if (!SD.begin(4)) {
    Serial.println("No se pudo inicializar");
    return;
  }
  Serial.println("inicializacion exitosa");

  
  delay(1000);
}

void loop()  {

 
  int m1[v][u]; 
  int m2[v][u];
  int m3[v][u];
  int m4[v][u];

while (j<1){
  for (w = 0; w < v; w++) //Estructura para crear filas
 {
    Serial.print("Vector de movimientos de los motores ");
    Serial.print(w+1);
    Serial.print(": [");

    for (z = 0; z < u; z++) //Estructura para crear columnas
    {


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

        m1[w][z] = regData5; //Creación del vector de las movimiento
        m2[w][z] = regData6;
        m3[w][z] = regData7;
        m4[w][z] = regData8;

        delay(20);

        Serial.print(m1[w][z]); 
        Serial.print(", ");
        Serial.print(m2[w][z]);
        Serial.print(", ");
        Serial.print(m3[w][z]);
        Serial.print(", ");
        Serial.print(m4[w][z]);
        Serial.print("; ");
        
        String dataString = ""; //vacia el string para evitar problemas
        myFile = SD.open("Prueba.txt", FILE_WRITE);//abrimos  el archivo

        if (myFile) {
        myFile.print(m1[w][z]); 
        myFile.print(", ");
        myFile.print(m2[w][z]);
        myFile.print(", ");
        myFile.print(m3[w][z]);
        myFile.print(", ");
        myFile.print(m4[w][z]);
        myFile.print("; ");

        
        
        }
        else {
        Serial.println("Error al abrir el archivo");
  }
    }  
 }
 myFile.close(); //cerramos el archivo
         delay(5000);
         Serial.println("FIN.");
         j++;
  }
}


Thank you,

Urko.
coder777 (8444)
I'm not sure what exactly you're asking?
If you ask where to save the values to the card: Right after you got them (after line 106).

Like always: I suggest that you do this in its own function.
urko18 (59)
what I want is to use the memory of the SD card instead of usin the memory of the microcontroller (that is full).

That is, now, iám saving the positions of the servos (line 71) in the memory of the micocontroller (ATMEGA644P), but this device doesn´t have memory to save more tha 400 positions.

If you ask where to save the values to the card:


Yes,that is what I want, but i don´t know how can i write this in the code, and also i don´t know once the values are in the SD, how i can read this values from the SD in order to do the procces between the lines 115-150.

doug4 (1538)
Generally an SD card is where we write an output file or read an input file. It sounds like you want to use the SD card as a kind of artificial RAM. Are you trying to use a file on the SD card as a place where you can read and write data for the running program? In other words, are you trying to use a file as essentially a really large array?

You can open an fstream (http://www.cplusplus.com/reference/fstream/fstream/) in binary mode and then use tellp/seekp and tellg/seekg to index into the file. Each integer value will take up sizeof(int) bytes, and you need to store 4 ints for each position. So, the nth (0-based) set of values will begin at byte [n * 4 * sizeof(int)]. Seek to this byte and read or write the 4 int values you are interested in.
urko18 (59)
Generally an SD card is where we write an output file or read an input file. It sounds like you want to use the SD card as a kind of artificial RAM. Are you trying to use a file on the SD card as a place where you can read and write data for the running program? In other words, are you trying to use a file as essentially a really large array?


That is what I want to do yes. The thing that I do not know how to read each position of the big array that i will have, that will be something like this: [2133, 3443, 4553, 3432; .... with much more positions],

I do not know if it will be better to pass the data to txt line by line, something like that

32133
2345
5456
3645
...


to later read them easier ...

Regarding the option that you mention to me, I think it can be a very good option even though I have not completely understood it, besides, the link does not work, I do not know if it will be bad or what happens, could you pass it to me again?

Thanks!

Urko
jonnin (11438)
another method would be to write to the card a block of commands in a group of files. then you would open file1, read it, do all those, reach the end, clear your memory and load file2, process it, ... etc. If you had a lot of complex operations you could make these files 'movement subroutines' and reuse them, eg if you had go here, pick up, go there, drop off, repeat 1000 times, you only need 2 or 3 files and you can just cycle through them with a pick up file and a drop off file etc.

use a binary file? It would use less space and be more efficient, and it is required if you want to do the seek/jump approach above but even if you do it with the multi-file idea it would be better. 255 is 3 bytes as text, 1 as a byte. This problem goes up rapidly as the values grow in size.
Last edited on
doug4 (1538)
This is sample code. I have not tested it, so there may be bugs in it.

To open a binary file for random access, use the following command:

1
2
3
4
 
std::ofstream binFile(fileName, 
	std::ios::binary |	// binary file
	std::ios::ate |		// enable writes at end of file but allow movement
	std::ios::in);		// enable reads



Assuming you always want to read/write positions for all 4 motors, use this struct to hold the data:

1
2
3
4
 
struct Positions
{
	int pos[servo_count];
};


To read the values from the file at "index"

1
2
3
4
 
Positions readPositions;
binFile.seekg(index * sizeof(Positions), std::ios_base::beg);
binfile.read(reinterpret_cast<char*>(&readPositions), sizeof(Positions));
// positions can now be found at readPositions.pos[0], readPositions[1], etc.


To write the values to the file at "index"

1
2
3
4
5
 
Positions writePositions;
// positions are stored at writePositions.pos[0], writePositions[1], etc.

binFile.seekp(index * sizeof(Positions), std::ios_base::beg);
binfile.read(reinterpret_cast<const char*>(&writePositions), sizeof(Positions));


Last edited on
urko18 (59)
Thank you very much!!

another method would be to write to the card a block of commands in a group of files. then you would open file1, read it, do all those, reach the end, clear your memory and load file2, process it, ... etc.


My idea is to be able to save a sequence of movements much larger than the maximum I can save right now. That is, my idea is at the beginning of the program to save the movements (right now on line 86, but my intention is to save it in the SD, line 97), and then read them from the SD and order the motors to move, such as doug4 said, use the SD as an artificial RAM.

I have managed to save the movements in the SD in the main program (Even though the program continues using the memory of the processor of the board):
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
 
#include <ax12.h> //Include ArbotiX DYNAMIXEL library
const int SERVO_ID[] = {1, 2, 3, 4}; 
const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID);
int repetitions;
double Speed;
int m = 100; //columns
int pos2[] = {1750, 400, 1500, 1330}; //rest position of the dynamixel motors
#include <SPI.h>
#include <SD.h>
File myFile;

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

  Serial.print("Inizializating SD ...");
  if (!SD.begin(4)) {
    Serial.println("error");
    return;
  }
  Serial.println("correct inizialization");

  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
  
  String dataString = "";
  myFile = SD.open("Prueba.txt", FILE_WRITE);//we open the file
  
  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(", "));
  
      if (myFile) {
      myFile.print(positionn[j][i]);
      myFile.print(F(", "));
      }
        else {
        Serial.println("Error opening the file");
      }
    }
  }
  Serial.print(F("]\n"));
  delay(5000);
  myFile.close(); //close the file
  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];
      position[i] = positionn[i][0];
      turns[i] = 0;
      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 (int 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(i + 1);
      Serial.print(F(": "));
      Serial.println(turns[i]);
      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);
    }
  }
}

As I understood, now I would have to convert that array [myFile.print(positionn[j][i]);(line 97)] into an array with binary numbers, and then with the sizeof () block, calculate how many positions it has and read them to put them in the program of lines 137-176, no?

I have been trying to integrate the blocks that you put me in the previous post in my program, but for now I have not achieved anything. I'll keep trying maybe in a simpler program.

Can you give me a example program or something like that to see better how those blocks works?

Thank you very much!

Urko
urko18 (59)
At the moment I have not managed to do anything ...

I just found this library that allows to use an external memory source to extend the amount of available RAM:


http://rhelmus.github.io/virtmem/index.html


Maybe I can use something like that ...

Did you know abaut this library?
coder777 (8444)
So this is a suggestion how SD files could work:
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
 
void loop()
{

...
  
  String dataString = "";
  // Note: make sure that he file is destroyed when opened
  // Note: I don't know whether FILE_READ | FILE_CREATE exists or named differently
  File myFile[servo_count] { SD.open("Prueba0.txt", FILE_WRITE | FILE_READ | FILE_CREATE), SD.open("Prueba1.txt", FILE_WRITE | FILE_READ | FILE_CREATE), ...}; // Note: 1 file per servo

  for(const File &f : myFile)
  {
    if(f)
        Serial.println("file is open");
    else
        Serial.println("Error opening the file");
  }

  for (int i = 0; i < m; i++) // structure to create columns
  {
    for (int j = 0; j < servo_count; j++) // structure to create columns
    {
      int pos = dxlGetPosition(SERVO_ID[j]);
      myFile[j].println(pos); //read and save the actual position

      Serial.print(pos); //Display the vector
      Serial.print(F(", "));
    }

    delay(10);
  }

...

  // Note: If the files cannot be opened for reading and writing or they do not have separate file pointers
  // The files need to be open again in read mode at this point

  for (int a = 0; a < repetitions; a++) //Repetition of the process (a = number of sequences)
  {
    Serial.print(F("SEQUENCE "));
    Serial.println(a + 1);

    int pos_from_file[servo_count]; // Note
    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++)
    {
      pos_from_file[i] = myFile[i].parseInt(); // Note
      current[i] = pos_from_file[i];
      position[i] = current[i];
      turns[i] = 0;
      pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
      pos2[i] = position[i]; //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 (int servo = 0; servo < servo_count; servo++)
      {
        if (pos_from_file[servo] != current[servo])
        {
          int next_pos = 1;
          if (pos_from_file[servo] < current[servo]) // Note
            next_pos = -1;
          while (pos_from_file[servo] != current[servo]) // Note
          {
            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]--;
            }
          }
        }
        pos_from_file[servo] = myFile[servo].parseInt(); // Note
      }
    }
...
  }

...

}
urko18 (59)
Thank you Coder777!!

Continuing with your advice, i have added this program that you had put in my program, doing small changes that I think that they have be done (like for example separate WRITE and READ blocks):
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
 
#include <ax12.h> //Include ArbotiX DYNAMIXEL library
const int SERVO_ID[] = {1, 2, 3, 4}; 
const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID);
int repetitions;
double Speed;
int m = 100; //columns
int pos2[] = {1750, 400, 1500, 1330}; //rest position of the dynamixel motors
#include <SPI.h>
#include <SD.h>
File myFile;

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

  Serial.print("Iniciando SD ...");
  if (!SD.begin(4)) {
    Serial.println("No se pudo inicializar");
    return;
  }
  Serial.println("inicializacion exitosa");

  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
  
  String dataString = ""; //vacia el string para evitar problemas

  myFile[servo_count] = { SD.open("Prueba0.txt", FILE_WRITE), SD.open("Prueba1.txt", FILE_WRITE), SD.open("Prueba2.txt", FILE_WRITE), SD.open("Prueba3.txt", FILE_WRITE)};

  for(const File &f : myFile)
  {
    if(f)
        Serial.println("file is open");
    else
        Serial.println("Error opening the file");
  }
  
  for (int i = 0; i < m; i++) // structure to create columns
  {
    for (int j = 0; j < servo_count; j++) // structure to create columns
    {
      int pos = dxlGetPosition(SERVO_ID[j]);
      myFile[j].println(pos); //read and save the actual position

      Serial.print(pos); //Display the vector
      Serial.print(F(", "));
    }
    delay(10);
  }
  
  Serial.print(F("]\n"));
  delay(5000);
  myFile.close(); //close the file

  
  Serial.println(F("The servos will move to the initial position."));
  /***The servos will move according to registered movements***/

  myFile[servo_count] = { SD.open("Prueba0.txt", FILE_READ), SD.open("Prueba1.txt", FILE_READ), SD.open("Prueba2.txt", FILE_READ), SD.open("Prueba3.txt", FILE_READ)};

    for (int a = 0; a < repetitions; a++) //Repetition of the process (a = number of sequences)
  {
    Serial.print(F("SEQUENCE "));
    Serial.println(a + 1);

    int pos_from_file[servo_count]; // Note
    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++)
    {
      pos_from_file[i] = myFile[i].parseInt(); // Note
      current[i] = pos_from_file[i];
      position[i] = current[i];
      turns[i] = 0;
      pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
      pos2[i] = position[i]; //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 (int servo = 0; servo < servo_count; servo++)
      {
        if (pos_from_file[servo] != current[servo])
        {
          int next_pos = 1;
          if (pos_from_file[servo] < current[servo]) // Note
            next_pos = -1;
          while (pos_from_file[servo] != current[servo]) // Note
          {
            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]--;
            }
          }
        }
        pos_from_file[servo] = myFile[servo].parseInt();
      }
    }
    for (int i = 0; i < servo_count; i++)
    {
      Serial.print(F("Turns engine "));
      Serial.print(i + 1);
      Serial.print(F(": "));
      Serial.println(turns[i]);
      Serial.println(" ");
    }
  }
  myFile.close(); //close the file
  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);
    }
  }
}

This is the program, which does not work, the errors that are display to me are the following:

error: no match for 'operator[]' (operand types are 'SDLib::File' and 'const int')

   myFile[servo_count] = { SD.open("Prueba0.txt", FILE_WRITE), SD.open("Prueba1.txt", FILE_WRITE), SD.open("Prueba2.txt", FILE_WRITE), SD.open("Prueba3.txt", FILE_WRITE)};

         ^

error: 'begin' was not declared in this scope

   for(const File &f : myFile)

                       ^

error: 'end' was not declared in this scope

error: no match for 'operator[]' (operand types are 'SDLib::File' and 'int')

       myFile[j].println(pos); //read and save the actual position

             ^

error: no match for 'operator[]' (operand types are 'SDLib::File' and 'const int')

   myFile[servo_count] = { SD.open("Prueba0.txt", FILE_READ), SD.open("Prueba1.txt", FILE_READ), SD.open("Prueba2.txt", FILE_READ), SD.open("Prueba3.txt", FILE_READ)};

         ^

error: no match for 'operator[]' (operand types are 'SDLib::File' and 'int')

       pos_from_file[i] = myFile[i].parseInt(); // Note

                                ^

error: no match for 'operator[]' (operand types are 'SDLib::File' and 'int')

         pos_from_file[servo] = myFile[servo].parseInt();

                                      ^


And honestly I do not know how to solve it ...

Maybe I could not do all that with a single file instead of one for each engine? I ask from my ignorance, because I am a beginner

Many thanks!!

Urko.
urko18 (59)
The thing is that I doubt that more than one file can be opened at the same time in the SD ...

I imagine it would be easier to keep all the movements in one ... the problem is that I do not know how i could take the positions from the file in order ...that is, first the ID1 motor, them, the ID2 motor...

doug4 (1538)
My second post covered this, but didn't state it explicitly.

Create a struct (struct Positions{...};) that contains a position for each motor. Then always read or write all 4 positions at the same time. So, you will always read/write (4 * sizeof(int)) bytes at the same time, and you will have fewer struggles trying to keep the data for the individual motors synchronized.
urko18 (59)
I'm sorry doug4 but I do not get to that level of programming, I do not understand how I can put those blocks that you put into my program ...

I assumed that in the structure
struct Positions {...};
is where the positions of the motors of my robot are collected? that is, that would be like when I use the code now this:
positionn [j] [i] = dxlGetPosition (SERVO_ID [j]); ???

Also I don´t understand what readpositions and writepositions do, are they supposed to be the same figures, right? I mean, the numbers that are written in the SD (writepositions) and the ones that have to be read afterwards from the SD are the same, so I do not understand why they are called different.

I do not know if I'm saying nonsense things because I do not quite understand the functioning of your blocks, the truth is that I'm not very skilled with Arduino.

If you would give me an example of how to use it or something like that, I would really appreciate it, because this morning I've been trying to put your blocks in my simplified program but not the truth that I did not understand what I was doing.

Many thanks!

Urko
Last edited on
jonnin (11438)
a struct in c++ is a user defined type. The most basic ones simply contain data, but you can make them full on objects with methods. But lets keep it simple.

struct fourints;
{
int motor1,motor2,motor3,motor4;
};

... //now we have a type.

fourints mymotorpositions[1000]; //now we have an array of 1000 user defined 4 ints.

mymotorpositions[0].motor1 = 1235; //this represents time slice 1 for motor 1.

mymotorpositions[100].motor2 = 3141; //this represents time slice 100 for motor 2

you can read and write such a thing in binary with a single statement...
//there may be more parameters to write, I forget, this is the idea

file.read(mymotorpositions, 1000*sizeof(fourints ));
file.write(mymotorpositions, 1000*sizeof(fourints ));

that is one way to do it.

another way to do it might be to do this:

enum{m1,m2,m3,m4}; //name your motors for readablity?
int positions[1000][4];

positions[timeincrement][m1] = value;
etc and you can read and write this the same way:

file.write(positions, sizeof(int)*4000);

either approach will work. what we are saying to do is this simple idea:
make a block of memory that can be read from or written to a file in one quick statement (1000 is just an example, make it the size you want).

to do that, you probably want an array (or vector but aurdino may not support) of 4000 (example) integers that represent 1000 (example) time slices and at each time slice, 4 motor positions. you can do that with a struct, or just a simple array. You can even do it with just a 1-d array but that is a bit ugly looking code and serves no purpose here.

you can typedef the array as a type if you want to make that a little prettier and if you need more than 1 of them at a time (they eat a lot of memory, so you can't have too many of the things).








coder777 (8444)
Your problem is that you treat myFile as an array while you didn't define it as such. You can actually use it without an array:
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
 
#include <ax12.h> //Include ArbotiX DYNAMIXEL library
const int SERVO_ID[] = {1, 2, 3, 4}; 
const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID);
int repetitions;
double Speed;
int m = 100; //columns
int pos2[] = {1750, 400, 1500, 1330}; //rest position of the dynamixel motors
#include <SPI.h>
#include <SD.h>
File myFile;

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

  Serial.print("Iniciando SD ...");
  if (!SD.begin(4)) {
    Serial.println("No se pudo inicializar");
    return;
  }
  Serial.println("inicializacion exitosa");

  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
  
  String dataString = ""; //vacia el string para evitar problemas

  myFile = SD.open("Prueba.txt", FILE_WRITE); // Note

    if(myFile)
        Serial.println("file is open");
    else
        Serial.println("Error opening the file");
  
  for (int i = 0; i < m; i++) // structure to create columns
  {
    for (int j = 0; j < servo_count; j++) // structure to create columns
    {
      int pos = dxlGetPosition(SERVO_ID[j]);
      myFile.println(pos); //read and save the actual position

      Serial.print(pos); //Display the vector
      Serial.print(F(", "));
    }
    delay(10);
  }
  
  Serial.print(F("]\n"));
  delay(5000);
  myFile.close(); //close the file

  
  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 pos_from_file[servo_count]; // Note
    int position[servo_count];
    int turns[servo_count];
    int pos1[servo_count];
    int pos2[servo_count];
    int current[servo_count];

  myFile = SD.open("Prueba.txt", FILE_READ); // Note
    
    for (int i = 0; i < servo_count; i++)
    {
      pos_from_file[i] = myFile.parseInt(); // Note
      current[i] = pos_from_file[i];
      position[i] = current[i];
      turns[i] = 0;
      pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
      pos2[i] = position[i]; //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 (int servo = 0; servo < servo_count; servo++)
      {
        if (pos_from_file[servo] != current[servo])
        {
          int next_pos = 1;
          if (pos_from_file[servo] < current[servo]) // Note
            next_pos = -1;
          while (pos_from_file[servo] != current[servo]) // Note
          {
            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]--;
            }
          }
        }
        pos_from_file[servo] = myFile.parseInt(); // Note
      }
    }
    for (int i = 0; i < servo_count; i++)
    {
      Serial.print(F("Turns engine "));
      Serial.print(i + 1);
      Serial.print(F(": "));
      Serial.println(turns[i]);
      Serial.println(" ");
    }
  }
  myFile.close(); //close the file
  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);
    }
  }
}
It is a matter of the correct order.
urko18 (59)
Thank you all for helping me and answering me!

I will respond in parts:

First, Jonnin, you have made me understand better how these blocks work, but even so I have not been able these days to introduce those pieces of code in my main program, I know it's a lot to ask but I think that if not you tell me where part of my code goes each block I will not be able to implement it, and I know it's my fault because I do not have much idea of programming...

to do that, you probably want an array (or vector but aurdino may not support) of 4000 (example) integers that represent 1000 (example) time slices and at each time slice, 4 motor positions. you can do that with a struct, or just a simple array

Yes, i want to do that, but if I do with struct, i don,t know how I have to add this structure in my main program, so, at the moment i can,t do that...

Second, Coder777, yes the code that you posted works correctly!!

Tomorrow I will have to see to see if the movements in the SD are actually being processed instead of in the microcontroller of the ArbotiX-M, since I am not sure at all, for this I will put in the variable M (line 6), more of 430 movements, which is the limit that I have of SRAM in the micro, tomorrow I will tell you!

Another problem that I have now is that I need once the program is run to erase the movements of the SD, since otherwise they are saved and they add in the file...
Any ideas?

Many thanks another time

Urko.
urko18 (59)
Definitely the program of coder777 works correctly!!

I have tried to put 1000 movements and the robot have repeated them correctly! after 5000 and also! It is a magnificent qualitative leap for the project! Then I tried to repeat the sequence of movements more than once and it also worked correctly.

But the program has a problem, the problem is that I need to delete the movements that are saved on the SD card, either at the beginning of each program (before picking up the new movements) or at the end of each program (once repeated sequence).

This is necessary since I have tried to repeat the program twice in a row without having deleted the movements, and the robot has repeated the sequence of the first program but not that of the second ... Any idea or any block of code to be able to do this?

Many thanks!!!

Urko.
urko18 (59)
Hellow again!


But the program has a problem, the problem is that I need to delete the movements that are saved on the SD card, either at the beginning of each program (before picking up the new movements) or at the end of each program (once repeated sequence).


I have alredy solved this problem, putting the following code at the end of the program:
1
2
3
4
5
6
 
   if (SD.exists("Prueba.txt")) {       
    SD.remove("Prueba.txt");          
    Serial.println("SD FILE REMOVED");
  } else {
    Serial.println("THE FILE DOESN´T EXIST");
  }


Urko.
Pages: 12