Need help for simultaneously rotate multiple servo with 'FOR' function loop

Projects Programming
1

so i have robotic arm project that implement an inverse kinematic formula from given input data of coordinate position of robot arm end-effector then through math equation (kinematic formula) to calculate the angle value for each joint need to rotate by servo motor. so i write the code using 'for' statement function, but the problem is the actual servo angle position is not the same as angle from calculation, the code is written like on the file attachment. (Note : the code file i attached is not the actual code, this is just parable from the actual code). then i run some 'for' statement loop experiment with three variables/condition that replicate scenario like my 3 servo on my robotic project, the result is like the screenshot on another file attachment.

or maybe is there any code writing tips to make each servo rotation is linear but on its own targeted angle position? let say servo_a trajectories is from 45 deg increment to 47 deg, servo_b from 76 deg decrement to 32 deg, servo_c from 12 deg increment to 145 deg. but all servo rotate simultaneously, so servo_c as the longest trajectory is the fastest, and servo_a as the shortest trajectory is the slowest because they need to done the rotation for the same amount of time.

Screenshot (33).png
Screenshot (33).png1366×768 107 KB

Example_Code.ino (1.25 KB)

Screenshot (33).png
Screenshot (33).png1366×768 107 KB

2

Firstly "function" has a very particular meaning, and the for-statement is not a function. Its a control-structure.

If you want to do several things at once, you cannot give control to just one of them, you have to interleave
their code using event-driven (aka state-machine) style of coding.

Then they can be fully independent.

You might want to check out Robin's thread on doing several things at once: Demonstration code for several things at the same time - Project Guidance - Arduino Forum

3

Your program is short so please include it in your post (and please use the code button </>) so we don't have to download it.

...R

4

Please follow the advice on posting a programming question given in Read this before posting a programming question

In particular note the advice to Auto format code in the IDE and to use code tags when posting code here

5 
#include <Servo.h>
#include <math.h>

#define Echo 7
#define Trigger 8

#define S0 A1
#define S1 A2
#define S2 A3
#define S3 A4
#define sensorOut A5

Servo servo1,servo2,servo3,servo4,gripper;

long Durasi;
int Jarak;

float min_PWM;
float max_PWM;

float Delay_MS = 50.000;

float Pi = 3.141592653589793;
float Yoffset;
float D;
float d;
float R;
float L1 = 64.50;
float L2 = 105.00;
float L3 = 98.50;
float L4 = 100.00;

float X_End_Effector,Y_End_Effector,Z_End_Effector;

int pin_servo1 = 5;
int pin_servo2 = 6;
int pin_servo3 = 9;
int pin_servo4 = 10;
int pin_gripper = 11;


float alpha1, alpha2, sudut_servo1, sudut_servo2, sudut_servo3, sudut_servo4;

float gerak_servo1 = 90.00;
float gerak_servo2 = 90.00;
float gerak_servo3 = 90.00;
float gerak_servo4 = 90.00;

void setup() 
{
  Serial.begin(9600);
  pinMode(Trigger, OUTPUT);
  pinMode(Echo, INPUT);
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);
  servo1.attach(pin_servo1, min_PWM = 500.0, max_PWM = 2500.00);
  servo2.attach(pin_servo2, min_PWM = 544.0, max_PWM = 544.00+(180.00/(190.00/(2000.00-544.00))));
  servo3.attach(pin_servo3, min_PWM = 544.0, max_PWM = 2030.00);
  servo4.attach(pin_servo4, min_PWM = 500.0, max_PWM = 2500.00);
  gripper.attach(pin_gripper);
}

void Rumus_IK()
{
  if (X_End_Effector > 0 && Y_End_Effector >= L1)
  {
    D = sqrt(pow(X_End_Effector,2) + pow(Z_End_Effector,2));
    sudut_servo3 = (atan(Z_End_Effector/X_End_Effector))*(180.00/Pi);
    d = D - L4;
    Yoffset = Y_End_Effector - L1;
    R = sqrt(pow(d,2) + pow(Yoffset,2));
    alpha1 = (acos(d/R))*(180.00/Pi);
    alpha2 = (acos((pow(L2,2) + pow(R,2) - pow(L3,2))/(2*L2*R)))*(180.00/Pi);
    sudut_servo1 = (alpha1 + alpha2);
    sudut_servo4 = ((acos((pow(L2,2) + pow(L3,2) - pow(R,2))/(2*L2*L3)))*(180.00/Pi));
    sudut_servo2 = 180.00 - ((180.00 - (alpha2 + sudut_servo4)) - alpha1);
  }
  else if (X_End_Effector > 0 && Y_End_Effector <= L1)
  {
    D = sqrt(pow(X_End_Effector,2) + pow(Z_End_Effector,2));
    sudut_servo3 = (atan(Z_End_Effector/X_End_Effector))*(180.00/Pi);
    d = D - L4;
    Yoffset = Y_End_Effector - L1;
    R = sqrt(pow(d,2) + pow(Yoffset,2));
    alpha1 = (acos(d/R))*(180.00/Pi);
    alpha2 = (acos((pow(L2,2) + pow(R,2) - pow(L3,2))/(2*L2*R)))*(180.00/Pi);
    sudut_servo1 = (alpha2 - alpha1);
    sudut_servo4 = ((acos((pow(L2,2) + pow(L3,2) - pow(R,2))/(2*L2*L3)))*(180.00/Pi));
    sudut_servo2 = 180.00 - ((180.00 - (alpha2 + sudut_servo4)) + alpha1);
  }
  else if (X_End_Effector == 0 && Y_End_Effector >= L1)
  {
    D = sqrt(pow(X_End_Effector,2) + pow(Z_End_Effector,2));
    sudut_servo3 = 90.00;
    d = D - L4;
    Yoffset = Y_End_Effector - L1;
    R = sqrt(pow(d,2) + pow(Yoffset,2));
    alpha1 = (acos(d/R))*(180.00/Pi);
    alpha2 = (acos((pow(L2,2) + pow(R,2) - pow(L3,2))/(2*L2*R)))*(180.00/Pi);
    sudut_servo1 = (alpha1 + alpha2);
    sudut_servo4 = ((acos((pow(L2,2) + pow(L3,2) - pow(R,2))/(2*L2*L3)))*(180.00/Pi));
    sudut_servo2 = 180.00 - ((180.00 - (alpha2 + sudut_servo4)) - alpha1);
  }
  else if (X_End_Effector == 0 && Y_End_Effector <= L1)
  {
    D = sqrt(pow(X_End_Effector,2) + pow(Z_End_Effector,2));
    sudut_servo3 = 90.00;
    d = D - L4;
    Yoffset = Y_End_Effector - L1;
    R = sqrt(pow(d,2) + pow(Yoffset,2));
    alpha1 = (acos(d/R))*(180.00/Pi);
    alpha2 = (acos((pow(L2,2) + pow(R,2) - pow(L3,2))/(2*L2*R)))*(180.00/Pi);
    sudut_servo1 = (alpha2 - alpha1);
    sudut_servo4 = ((acos((pow(L2,2) + pow(L3,2) - pow(R,2))/(2*L2*L3)))*(180.00/Pi));
    sudut_servo2 = 180.00 - ((180.00 - (alpha2 + sudut_servo4)) + alpha1);
  }
  else if (X_End_Effector < 0 && Y_End_Effector >= L1)
  {
    D = sqrt(pow(X_End_Effector,2) + pow(Z_End_Effector,2));
    sudut_servo3 = 90.00 + (90 - abs((atan(Z_End_Effector/X_End_Effector))*(180.00/Pi)));
    d = D - L4;
    Yoffset = Y_End_Effector - L1;
    R = sqrt(pow(d,2) + pow(Yoffset,2));
    alpha1 = (acos(d/R))*(180.00/Pi);
    alpha2 = (acos((pow(L2,2) + pow(R,2) - pow(L3,2))/(2*L2*R)))*(180.00/Pi);
    sudut_servo1 = (alpha1 + alpha2);
    sudut_servo4 = ((acos((pow(L2,2) + pow(L3,2) - pow(R,2))/(2*L2*L3)))*(180.00/Pi));
    sudut_servo2 = 180.00 - ((180.00 - (alpha2 + sudut_servo4)) - alpha1);
  }
  else if (X_End_Effector < 0 && Y_End_Effector <= L1)
  {
    D = sqrt(pow(X_End_Effector,2) + pow(Z_End_Effector,2));
    sudut_servo3 = 90.00 + (90.00 - abs((atan(Z_End_Effector/X_End_Effector))*(180.00/Pi)));
    d = D - L4;
    Yoffset = Y_End_Effector - L1;
    R = sqrt(pow(d,2) + pow(Yoffset,2));
    alpha1 = (acos(d/R))*(180.00/Pi);
    alpha2 = (acos((pow(L2,2) + pow(R,2) - pow(L3,2))/(2*L2*R)))*(180.00/Pi);
    sudut_servo1 = (alpha2 - alpha1);
    sudut_servo4 = ((acos((pow(L2,2) + pow(L3,2) - pow(R,2))/(2*L2*L3)))*(180.00/Pi));
    sudut_servo2 = 180.00 - ((180.00 - (alpha2 + sudut_servo4)) + alpha1);
  }
}

void gerak_increment_servo3()
{
  Rumus_IK();
  for(gerak_servo3 == sudut_servo3; gerak_servo3 <= sudut_servo3; gerak_servo3 += 0.01)
  {
    servo3.write(gerak_servo3);
    delayMicroseconds(Delay_MS);
  }
  for(gerak_servo3 == sudut_servo3; gerak_servo3 >= sudut_servo3; gerak_servo3 -= 0.01)
  {
    servo3.write(gerak_servo3);
    delayMicroseconds(Delay_MS);
  }
}

void gerak_increment_servo1()
{
  Rumus_IK();
  for(gerak_servo1 == sudut_servo1; gerak_servo1 <= sudut_servo1; gerak_servo1 += 0.01)
  {
    servo1.write(gerak_servo1);
    delayMicroseconds(Delay_MS);
  }
  for(gerak_servo1 == sudut_servo1; gerak_servo1 >= sudut_servo1; gerak_servo1 -= 0.01)
  {
    servo1.write(gerak_servo1);
    delayMicroseconds(Delay_MS);
  }
}

void gerak_increment_servo4()
{
  Rumus_IK();
  for(gerak_servo4 == sudut_servo4; gerak_servo4 <= sudut_servo4; gerak_servo4 += 0.01)
  {
    servo4.write(gerak_servo4);
    delayMicroseconds(Delay_MS);
  }
  for(gerak_servo4 == sudut_servo4; gerak_servo4 >= sudut_servo4; gerak_servo4 -= 0.01)
  {
    servo4.write(gerak_servo4);
    delayMicroseconds(Delay_MS);
  }
}

void gerak_increment_servo2()
{
  Rumus_IK();
  for(gerak_servo2 == (sudut_servo2 - 90.00); gerak_servo2 <= (sudut_servo2 - 90.00); gerak_servo2 += 0.01)
  {
    servo2.write(gerak_servo2);
    delayMicroseconds(Delay_MS);
  }
  for(gerak_servo2 == (sudut_servo2 - 90.00); gerak_servo2 >= (sudut_servo2 - 90.00); gerak_servo2 -= 0.01)
  {
    servo2.write(gerak_servo2);
    delayMicroseconds(Delay_MS);
  }
}

void gerak_increment_servo_1_4_2()
{
  Rumus_IK();
  for(gerak_servo1 = sudut_servo1, gerak_servo4 = sudut_servo4; gerak_servo1 <= sudut_servo1 && gerak_servo4 <= sudut_servo4; gerak_servo1 += 0.01, gerak_servo4 += 0.01)
  {
    servo1.write(gerak_servo1);
    servo4.write(gerak_servo4);
    delayMicroseconds(Delay_MS);
  }
  for(gerak_servo1 = sudut_servo1, gerak_servo4 = sudut_servo4; gerak_servo1 >= sudut_servo1 && gerak_servo4 >= sudut_servo4; gerak_servo1 -= 0.01, gerak_servo4 -= 0.01)
  {
    servo1.write(gerak_servo1);
    servo4.write(gerak_servo4);
    delayMicroseconds(Delay_MS);
  }
}

void loop()
{
  digitalWrite(Trigger, LOW);
  delayMicroseconds(2);
  digitalWrite(Trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trigger, LOW);
  Durasi = pulseIn(Echo, HIGH);
  Jarak = Durasi*0.034/2;
  if(Jarak >= 10)//posisi home, saat ga ada objek
  {
    gripper.write(150);
    X_End_Effector = 100.00;
    Y_End_Effector = 200.00;
    Z_End_Effector = 5.00;
    gerak_increment_servo1();
    gerak_increment_servo4();
    gerak_increment_servo2();
    //gerak_increment_servo_1_4_2();
    gerak_increment_servo3();
  }
  else if(Jarak < 10)
  {
    {//ambil(masih di atas) 
      X_End_Effector = 10.00;
      Y_End_Effector = 100.00;
      Z_End_Effector = 200.00;
      gerak_increment_servo3();
      gerak_increment_servo2();
      gerak_increment_servo4();
      gerak_increment_servo1();
      //gerak_increment_servo_1_4_2();
    }
  }
}
6

As @MarkT has said in Reply #1, you need to interleave the motion of all the servos - something like

struct ServoStruct {
    byte servoPin;
    int step;
    int endPosition;
    int curPosition;
    Servo servo;
};

ServoStruct myServo[4] = {
                            {3, 12, 1800, 1200},
                            {4, 12, 1800, 1200},
                            {5, 12, 1800, 1200},
                            {6, 12, 1800, 1200}
                        };


void updateServoPositions() {
    for (byte n = 0; n < 1; n++) {
        if (myServo[n].curPosition < myServo[n].endPosition) {
            myServo[n].curPosition += myServo[n].step;
            if (myServo[n].curPosition > myServo[n].endPosition) {
                myServo[n].curPosition = myServo[n].endPosition;
            }
            myServo[n].servo.write(myServo[n].curPosition);
        }
    }
}

I reckon it makes the code easier if the servo data is put into a struct rather than creating 3 or 4 different arrays. I have checked that this compiles but I have not tested it.

...R

7

You need to set up a map() function tuned for each servo. What they do is "map" your raw angle to the parameter the servo wants to see to give you that result.

-jim lee