How store servos movements in a microSD

Hellow!!

I'm doing the programming of a robotic arm (maybe some of them sound like 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 of the robot (I manually move the robot), and after, the robot repeats the movements that I have done previously.

#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, interrupt_state;
double Speed;
int m = 430; //columns
int pos2[] = {3820, 400, 2075, 680}; //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 the vector of:

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

I have simplified the program a lot and I have managed to pass the movements to the SD:

#include <ax12.h> //Incluir la librería ArbotiX DYNAMIXEL
int SERVO_ID[] = {1, 2, 3, 4}; //números ID de los motores que se están utilizando
const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID);
int m = 200; //columnas -> cantidad de posiciones guardadas por cada movimiento

#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("Starting SD ...");
  if (!SD.begin(4)) {
    Serial.println("It failed to initialize");
    return;
  }
  Serial.println("successful initialization");

  
  delay(1000);
}

void loop()  {
  
 while (1) {
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(", "));

      String dataString = ""; 
      myFile = SD.open("Prueba.txt", FILE_WRITE);
      
      if (myFile) {
      myFile.print(positionn[j][i]);
      myFile.print(F(", "));

        myFile.close(); 
      }
        else {
        Serial.println("Error opening the file");
  }
    }
  }
  Serial.println(F("END!"));
 }
}

but, now, I want to integrate this part of the program in which I have taught you in the previous post.

You saved the movements, but you did not save the time the movement should take. So when you replay the movement file, the whole thing will run in less than a second.

Paul

Thank you for answering!

I put after I save the movements a delay of 10 ms, i´m not sure if you are saying that.

I have tested to put a delay after I print the torques but the program do the same...

So, I still working in this

Urko.

urko_18:
Thank you for answering!

I put after I save the movements a delay of 10 ms, i´m not sure if you are saying that.

I have tested to put a delay after I print the torques but the program do the same...

So, I still working in this

Urko.

I am saying "You have to know and store the amount of time each movements takes in order to properly order the movements when you are running from the stored file". That way you don't order the next movement before the current movement has completed. Unless that is what you want.

Paul

Okay, I do not know if I understand you at all, so, I'm going to start going step by step:

First, when I save the array of the positions of the motors on the SD card (something like this: [2344, 4567, 6353, 5335, ...]), when I want to open the file , how can I know how many positions there are? ? Could a sizeof () be used?

Also, how could I control how often the positions are saved in the SD?

Thank you,

Urko

urko_18:
Okay, I do not know if I understand you at all, so, I'm going to start going step by step:

First, when I save the array of the positions of the motors on the SD card (something like this: [2344, 4567, 6353, 5335, ...]), when I want to open the file , how can I know how many positions there are? ? Could a sizeof () be used?

Also, how could I control how often the positions are saved in the SD?

Thank you,

Urko

Since you don't know when each movement has been completed, and use delay to make a default time, store the movement code and the delay time you used. And store to the SD card at the end of each movement.

Then when read back, you have the movement and the time to delay before the next movement.

Why do you care how many many movements there are? If you want to know when the last movement has been done, make the last movement code all 9's, or something that will not be an actual movement.

Paul

Use rotary encoders to record the positions. They track movement on the shaft.

Thanks to all of you!

Finally I have solve the problem by using the SD.h library (correctly).

Basically, i have used the followings codes and blocks:

To start the conection between the SD and the PC-Arduino

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

This to open/create a file and to save there the movements of the servos and them to close the file:

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

    if(myFile)
        Serial.println("file is open");
    else
        Serial.println("Error opening the file");

...

int pos = dxlGetPosition(SERVO_ID[j]);
      myFile.println(pos); //read and save the actual position
...
myFile.close(); //close the file

I have add a new variable in the program (this variable datas come from the SD), and I have used like the variable of the positionn that I have put in the previous posts, to read te datas i have used myFile.parseint:.

int pos_from_file[servo_count]; 
...
pos_from_file[i] = myFile.parseInt();

And finally this block to delete the file at the end of the program:

   if (SD.exists("Prueba.txt")) {        // SI EXISTE EL ARCHIVO
    SD.remove("Prueba.txt");           // ELIMINAR ARCHIVO
    Serial.println("SD FILE REMOVED");
  } else {
    Serial.println("THE FILE DOESN´T EXIST");
  }

Congrats!!!