How to reduce code of an arduino program of a robotic arm

Hi everyone!

I am programing in arduino a control of a robotic arm with 4 motors, and I have the same code of movements for each motor, each one with his number variable.

For example, this part of the code corresponds to the 2 motor:

//ID2

if (positionn2[l] < initial2)

  • {*
    _ while (positionn2[l] < initial2)_
    * {*
    * before_position_2 = initial2;
    dxlSetGoalPosition(SERVO_ID[1], initial2);
    _
    initial2--;_
    _
    delayMicroseconds(400);_
    if (initial2 > before_position_2 || initial2 < before_position_2)
    _
    {_
    if (initial2 == position2 + MX_MAX_POSITION_VALUE)
    _
    {_
    _
    position2 = initial2;_
    _
    turns2++;_
    _
    Serial.print("Vueltas 2: ");_
    _
    Serial.println(turns2);_
    _
    }_
    else if (initial2 == position2 - MX_MAX_POSITION_VALUE)
    _
    {_
    _
    position2 = initial2;_
    _
    turns2--;_
    _
    Serial.print("Vueltas 2: ");_
    _
    Serial.println(turns2);_
    _
    }_
    _
    }_
    else if (initial2 == before_position_2)
    _
    {_
    _
    turns2 = turns2;_
    _
    }_
    _
    }_
    _
    }_
    _ else if (positionn2[l] > initial2)
    {
    while (positionn2[l] > initial2)
    {_

    before_position_2 = initial2;
    dxlSetGoalPosition(SERVO_ID[1], initial2);
    _ initial2++;
    delayMicroseconds(400);_

    if (initial2 > before_position_2 || initial2 < before_position_2)
    _ {_
    if (initial2 == position2 + MX_MAX_POSITION_VALUE)
    _ {
    position2 = initial2;
    turns2++;
    Serial.print("Vueltas 2: ");
    Serial.println(turns2);
    }_

    else if (initial2 == position2 - MX_MAX_POSITION_VALUE)
    _ {
    position2 = initial2;
    turns2--;
    Serial.print("Vueltas 2: ");
    Serial.println(turns2);
    }
    }_

    else if (initial2 == before_position_2)
    _ {
    turns2 = turns2;
    }
    }
    }*

    it is posible to reduce the code? that is, is posible instead of put the same code to each motor (changing the numbers) to put all together in one code "part" in order to clarify and become easier to read the program?
    Thank you!_

it is posible to reduce the code?

It is possible to use arrays of pin numbers, desired positions, etc., and pass those arrays, long with an index telling the function which element in the arrays is important, to a function that can move a motor. You can then call that function in a for loop, to move all the motors.

I can't really see how your code is going to move a robot arm in a way that even remotely resembles how a real robot arm moves. A real robot arm does not move one joint at a time.

Thank you for answering PaulS!!

Maybe I've expressed myself badly in English, I'm sorry. What I want to know is if the following code (where the motors make some movements already saved in the program (in a vector)) can be reduced to a single function.

 //ID1
  if (positionn1[l][i] < initial1)
  {
   while (positionn1[l][i] < initial1)
   {
    before_position_1 = initial1;
    dxlSetGoalPosition(SERVO_ID[0], initial1);
    initial1--;
    delayMicroseconds(400);

    if (initial1 > before_position_1 || initial1 < before_position_1)
    {
      if (initial1 == position1 + MX_MAX_POSITION_VALUE)
      {
        position1 = initial1;
        turns++;
        Serial.print("Turns: ");
        Serial.println(turns);
      }
      else if (initial1 == position1 - MX_MAX_POSITION_VALUE)
      {
        position1 = initial1;
        turns--;
        Serial.print("Turns: ");
        Serial.println(turns);
      }
    }
    else if (initial1 == before_position_1)
    {
      turns = turns;
    }
   }
  }
  else if (positionn1[l][i] > initial1)
  {
   while (positionn1[l][i] > initial1)
   {
    before_position_1 = initial1;
    dxlSetGoalPosition(SERVO_ID[0], initial1);
    initial1++;
    delayMicroseconds(400);
    if (initial1 > before_position_1 || initial1 < before_position_1)
    {
      if (initial1 == position1 + MX_MAX_POSITION_VALUE)
      {
        position1 = initial1;
        turns++;
        Serial.print("Turns: ");
        Serial.println(turns);
      }
      else if (initial1 == position1 - MX_MAX_POSITION_VALUE)
      {
        position1 = initial1;
        turns--;
        Serial.print("Turns: ");
        Serial.println(turns);
      }
    }
    else if (initial1 == before_position_1)
    {
      turns = turns;
    }
   }
  }

  //ID2
  if (positionn2[l][i] < initial2)
  {
   while (positionn2[l][i] < initial2)
   {
    before_position_2 = initial2;
    dxlSetGoalPosition(SERVO_ID[1], initial2);
    initial2--;
    delayMicroseconds(400);

     if (initial2 > before_position_2 || initial2 < before_position_2)
     {
      if (initial2 == position2 + MX_MAX_POSITION_VALUE)
      {
        position2 = initial2;
        turns2++;
        Serial.print("Vueltas 2: ");
        Serial.println(turns2);
      }
      else if (initial2 == position2 - MX_MAX_POSITION_VALUE)
      {
        position2 = initial2;
        turns2--;
        Serial.print("Vueltas 2: ");
        Serial.println(turns2);
      }
    }
    else if (initial2 == before_position_2)
    {
      turns2 = turns2;
    }
   }
  }
  else if (positionn2[l][i] > initial2)
  {
   while (positionn2[l][i] > initial2)
   {
    before_position_2 = initial2;
    dxlSetGoalPosition(SERVO_ID[1], initial2);
    initial2++;
    delayMicroseconds(400);

    if (initial2 > before_position_2 || initial2 < before_position_2)
    {
      if (initial2 == position2 + MX_MAX_POSITION_VALUE)
      {
        position2 = initial2;
        turns2++;
        Serial.print("Vueltas 2: ");
        Serial.println(turns2);
      }
      else if (initial2 == position2 - MX_MAX_POSITION_VALUE)
      {
        position2 = initial2;
        turns2--;
        Serial.print("Vueltas 2: ");
        Serial.println(turns2);
      }
    }
    else if (initial2 == before_position_2)
    {
      turns2 = turns2;
    }
   }
  }

  //ID3
  if (positionn3[l][i] < initial3)
  {
   while (positionn3[l][i] < initial3)
   {
    before_position_3 = initial3;
    dxlSetGoalPosition(SERVO_ID[2], initial3);
    initial3--;
    delayMicroseconds(400);

    if (initial3 > before_position_3 || initial3 < before_position_3)
    {
      if (initial3 == position3 + MX_MAX_POSITION_VALUE)
      {
        position3 = initial3;
        turns3++;
        Serial.print("Vueltas 3: ");
        Serial.println(turns3);
      }
      else if (initial3 == position3 - MX_MAX_POSITION_VALUE)
      {
        position3 = initial3;
        turns3--;
        Serial.print("Vueltas 3: ");
        Serial.println(turns3);
      }
    }
    else if (initial3 == before_position_3)
    {
      turns3 = turns3;
    }
   }
  }
  else if (positionn3[l][i] > initial3)
  {
   while (positionn3[l][i] > initial3)
   {
    before_position_3 = initial3;
    dxlSetGoalPosition(SERVO_ID[2], initial3);
    initial3++;
    delayMicroseconds(400);

    if (initial3 > before_position_3 || initial3 < before_position_3)
    {
      if (initial3 == position3 + MX_MAX_POSITION_VALUE)
      {
        position3 = initial3;
        turns3++;
        Serial.print("Vueltas 3: ");
        Serial.println(turns3);
      }
      else if (initial3 == position3 - MX_MAX_POSITION_VALUE)
      {
        position3 = initial3;
        turns3--;
        Serial.print("Vueltas 3: ");
        Serial.println(turns3);
      }
    }
    else if (initial3 == before_position_3)
    {
      turns3 = turns3;
    }
   }
  }

  //ID4
  if (positionn4[l][i] < initial4)
  {
   while (positionn4[l][i] < initial4)
   {
    before_position_4 = initial4;
    dxlSetGoalPosition(SERVO_ID[3], initial4);
    initial4--;
    delayMicroseconds(400);

    if (initial4 > before_position_4 || initial4 < before_position_4)
    {
      if (initial4 == position4 + MX_MAX_POSITION_VALUE)
      {
        position4 = initial4;
        turns4++;
        Serial.print("Vueltas 4: ");
        Serial.println(turns4);
      }
      else if (initial4 == position4 - MX_MAX_POSITION_VALUE)
      {
        position3 = initial4;
        turns4--;
        Serial.print("Vueltas 4: ");
        Serial.println(turns4);
      }
    }
    else if (initial4 == before_position_4)
    {
      turns4 = turns4;
    }
   }
  }
  else if (positionn4[l][i] > initial4)
  {
   while (positionn4[l][i] > initial4)
   {
    before_position_4 = initial4;
    dxlSetGoalPosition(SERVO_ID[3], initial4);
    initial4++;
    delayMicroseconds(400);

    if (initial4 > before_position_4 || initial4 < before_position_4)
    {
      if (initial4 == position4 + MX_MAX_POSITION_VALUE)
      {
        position4 = initial4;
        turns4++;
        Serial.print("Vueltas 4: ");
        Serial.println(turns4);
      }
      else if (initial4 == position4 - MX_MAX_POSITION_VALUE)
      {
        position4 = initial4;
        turns4--;
        Serial.print("Vueltas 4: ");
        Serial.println(turns4);
      }
    }
    else if (initial4 == before_position_4)
    {
      turns4 = turns4;
    }
   }
  }
 }

I´m trying to do this:

 void repeat_sequence(double initial, double turns, double before_position, double positionn, double position);
    {
    repeat_sequence(initial1, turns1, before_position_1, positionn1, position1);
    repeat_sequence(initial2, turns2, before_position_2, positionn2, position2);
    repeat_sequence(initial3, turns3, before_position_3, positionn3, position3);
    repeat_sequence(initial4, turns4, before_position_4, positionn4, position4);
 
  if (positionn[l][i] < initial)
  {
   while (positionn[l][i] < initial)
   {
    before_position = initial;
    dxlSetGoalPosition(SERVO_ID[0], initial);
    initial--;
    delayMicroseconds(400);

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

But the program gives some errors, I have one question, is it posible to pass Arrays into a function? Because this is the error that the program gives:

cannot convert 'int (*)[m]' to 'double' for argument '4' to 'void repeat_sequence(double, double, double, double, double)'.

Other question, do I have to declare turns, position, positionn, before_position...???

Or there si any other way to do it?

PaulS:
It is possible to use arrays of pin numbers, desired positions, etc., and pass those arrays, long with an index telling the function which element in the arrays is important, to a function that can move a motor. You can then call that function in a for loop, to move all the motors.

I can't really see how your code is going to move a robot arm in a way that even remotely resembles how a real robot arm moves. A real robot arm does not move one joint at a time.

If they reduce the duty cycle of each discrete motor's power on time and there's nothing else blocking program flow, e.g. interrupts, buttons, slow sensors, etc., shouldn't the robot's movement appear to be smooth even when multiple motors are used? I.e PWM for the motors? That's how a robot arm project I've been working on, that uses 5 discrete motors each controlling a separate axis of movement, works, and it simulates smooth multi-motor movement pretty well.

That said, I agree that using some sort of mapping array and function is the way to go, or at least switch statements. I'm using the former, with good results. In additional to making the code much smaller and easier to follow, it allows me to introduce custom parameters for each motor, e.g. duty cycle, time on, etc.

you give it a INTeger and the program asks for a double.