5-DOF arm robot issues

Hi,
I am trying to use a 5-DOF arm robot to pick and place some objects. I am having a problem with the servo motors' movements, when trying to use serial to move the robot to different positions, the servo robot is moving very fast before it executes the order I sent via serial. Has anyone encountered a similar issue before? here is my code below:

#include <Servo.h>
#include <EEPROM.h>


// Define initial positions for each servo motor
int N = 1;
double sgn(double x);
float y[4] = {0, 0, 0, 0}; //lspb variables
float tdjk = 500; //time interval microseconds
float acc = 0.0002; //acceleration,
double ti[4] = {0, 0, 0, 0};
double tjkk[4] = {0, 0, 0, 0};
double tdijo[4] = {tdjk, tdjk, tdjk, tdjk};
double tdijp[4] = {tdjk, tdjk, tdjk, tdjk};
float tk[4] = {0, 0, 0, 0};
float tj[4] = {0, 0, 0, 0};
float tjk[4] = {0, 0, 0, 0};
float mi[4] = {0, 0, 0, 0};
float mf[4] = {0, 0, 0, 0};
float ak[4] = {0, 0, 0, 0};
float a[4] = {0, 0, 0, 0};
float b[4] = {0, 0, 0, 0};
float c[4] = {0, 0, 0, 0};
int m[4] = {0, 0, 0, 0}; //blend
int l[4] = {0, 0, 0, 0}; //linear
int o[4] = {0, 0, 0, 0};
int p[4] = {0, 0, 0, 0};
unsigned long t[4] = {0, 0, 0, 0};
unsigned long td[4] = {0, 0, 0, 0};
int n = 0;
float qu[4] = {0, 0, 0, 0}; //counters
float qv[4] = {0, 0, 0, 0};
float qw[4] = {0, 0, 0, 0};
float qc[4] = {0, 0, 0, 0};
float qum = 0;
float qvm = 0;
float qwm = 0;
float qcm = 0;
int uc = 0;
int vc = 0;
int wc = 0;
int cc = 0;

int z0 = 0; //z offset for target
uint8_t currentPosition; // Declare currentPosition as uint8_t = 1; // Variable to store the current position

// create four servo objects

Servo j11;
Servo j2;
Servo j3;
Servo j4;
Servo j5;
Servo j6;
Servo g6;
int d1 = 90, d4 = 121, d6 = 0, l0 = 28, l2 = 150; //dh parameters
float q1, q2, q3, q6;
float u(float t06[12][4][4]); //inverse kinematics functions
float v(float t06[12][4][4]);
float w(float t06[12][4][4]);
float g(float t06[12][4][4]);
void parabolicBlend( int q1, int q2); //linear segment with parabolic blend function

float t07[][4][4] = {{{1, 0, 0, 0}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}}, //target sequence
  {{1, 0, 0, 0}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 45 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, -120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 70 - z0}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 120}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 0}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 0}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 0}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}},
  {{1, 0, 0, 0}, {0, 0, -1, 170}, {0, 1, 0, 240}, {0, 0, 0, 1}}
};
int targetPosition ; // Initialize target position variable

void setup() {
  Serial.begin(9600);
  j11.attach(2);
  j2.attach(3);
  j3.attach(4);
  j6.attach(5);
  g6.attach(6);
  g6.write(0);

  EEPROM.get(10, currentPosition);
  Serial.println(currentPosition);
  
  delay(3000); // Additional delay after startup routine

}



void loop()

{

  if (Serial.available() > 0) { // Check if data is available to read


    if (Serial.peek() != '-') { // Check if the next character is not '-' (negative sign)
      targetPosition = Serial.parseInt(); // Read the target position from serial
      Serial.read(); // Consume the newline character

      // Move to the target position using parabolicBlend
      Serial.println(targetPosition);
      currentPosition = currentPosition - 1;
      parabolicBlend(currentPosition, targetPosition);
      delay(1000);


      
      currentPosition = targetPosition;
      Serial.println(currentPosition);
      //targetPosition=targetPosition+9;
      EEPROM.put(10, currentPosition);

      // Optionally, send feedback that movement is complete
      //delay(1000);
      Serial.println("Movement complete");
    }
  }
  //delay(1000);

}


void parabolicBlend( int q1, int q2)
{
  for (n = 0; n < 4; n++)
  {
    ti[n] = millis();
    m[n] = q1;
    l[n] = q1;
  }

  qu[0] = u(t07, l[n]);
  qu[1] = u(t07, (l[n] + 1));
  qu[2] = u(t07, (l[n] + 2));

  qv[0] = v(t07, l[n]);
  qv[1] = v(t07, (l[n] + 1));
  qv[2] = v(t07, (l[n] + 2));

  qw[0] = w(t07, l[n]);
  qw[1] = w(t07, (l[n] + 1));
  qw[2] = w(t07, (l[n] + 2));

  qc[0] = g(t07, l[n]);
  qc[1] = g(t07, (l[n] + 1));
  qc[2] = g(t07, (l[n] + 2));

  uc = l[n];
  vc = l[n];
  wc = l[n];
  cc = l[n];

  while (l[3] <= q2)
  {
    for (n = 0; n < 4; n++)
    {
      t[n] = millis();
      td[n] = t[n] - ti[n];

      switch (n)
      {
        case 0:
          if (uc != l[n]) //if change in l[n] then find new qv values
          {
            qu[0] = qu[1];
            qu[1] = qu[2];
            qu[2] = u(t07, (l[n] + 2));
            uc++;
          }
          mi[n] = (qu[1] - qu[0]) / (float)tdjk; //final-intial, slope of initial, inverse kinematics here
          mf[n] = (qu[2] - qu[1]) / (float)tdjk; //final slope, inverse kinematics her
          break;

        case 1:
          if (vc != l[n]) //if change in l[n] then find new qv values
          {
            qv[0] = qv[1];
            qv[1] = qv[2];
            qv[2] = v(t07, (l[n] + 2));
            vc++;
          }
          mi[n] = (qv[1] - qv[0]) / (float)tdjk; //final-intial, slope of initial, inverse kinematics here
          mf[n] = (qv[2] - qv[1]) / (float)tdjk; //final slope, inverse kinematics her
          break;

        case 2:
          if (wc != l[n]) //if change in l[n] then find new qv values
          {
            qw[0] = qw[1];
            qw[1] = qw[2];
            qw[2] = w(t07, (l[n] + 2));
            wc++;
          }
          mi[n] = (qw[1] - qw[0]) / (float)tdjk; //final-intial, slope of initial, inverse kinematics here
          mf[n] = (qw[2] - qw[1]) / (float)tdjk; //final slope, inverse kinematics her
          break;

        case 3:
          if (cc != l[n]) //if change in l[n] then find new qv values
          {
            qc[0] = qc[1];
            qc[1] = qc[2];
            qc[2] = g(t07, (l[n] + 2));
            cc++;
          }
          mi[n] = (qc[1] - qc[0]) / (float)tdjk; //final-intial, slope of initial, inverse kinematics here
          mf[n] = (qc[2] - qc[1]) / (float)tdjk; //final slope, inverse kinematics her
          break;

        default:
          break;
      }

      if (mi[n] == mf[n])
      {
        if (t[n] > (tjkk[n] + ti[n]) && t[n] <= (tdijo[n] + ti[n]))
        {
          switch (n)
          {
            case 0:
              y[n] = qu[0];
              break;

            case 1:
              y[n] = qv[0];
              break;

            case 2:
              y[n] = qw[0];
              break;

            case 3:
              y[n] = qc[0];
              break;

            default:
              break;
          }
        }

        if (t[n] > (tdijo[n] + ti[n]))
        {
          tjkk[n] = tdijo[n];
          tdijo[n] += tdjk; //tjkk[n]= tdjk*(o[n]+1);
          l[n]++;
          tj[n] = 0;
        }
      }

      else
      {
        ak[n] = sgn(mf[n] - mi[n]) * acc; //acceleration
        tk[n] = (mf[n] - mi[n]) / ak[n]; //if mf[n]-mi[n]=0 then see tk=0
        tjk[n] = tdjk - (tk[n] / 2) - (tj[n] / 2);

        if (t[n] > (tjk[n] + tjkk[n] + ti[n]) && t[n] < (abs(tk[n]) + tjk[n] + tjkk[n] + ti[n])) //blend part
        {
          a[n] = ak[n] / 2;
          b[n] = mi[n];

          switch (n)
          {
            case 0:
              c[n] = qu[1] - (mi[n] * tk[n] / 2);
              break;

            case 1:
              c[n] = qv[1] - (mi[n] * tk[n] / 2);
              break;

            case 2:
              c[n] = qw[1] - (mi[n] * tk[n] / 2);
              break;

            case 3:
              c[n] = qc[1] - (mi[n] * tk[n] / 2);
              break;

            default:
              break;
          }
          y[n] = (a[n] * (float(td[n]) - tjkk[n] - tjk[n]) * (float(td[n]) - tjkk[n] - tjk[n])) + (b[n] * (float(td[n]) - tjkk[n] - tjk[n])) + c[n]; //y[n]= (a[n]*pow((t[n]-tjkk[n]-tjk[n]),2))+(b[n]*(t[n]-tjkk[n]-tjk[n]))+c[n];
        }

        if (t[n] >= (abs(tk[n]) + tjk[n] + tjkk[n] + ti[n])) //(t[n]> (abs(tk[n])+tjk[n]+tjkk[n]))
        {
          tjkk[n] = tjk[n] + tk[n] + tjkk[n];
          tj[n] = tk[n];
          tdijo[n] += tdjk;
          l[n]++;
        }

        if (t[n] <= (tjk[n] + tjkk[n]) + ti[n]) //linear part
        {
          switch (n)
          {
            case 0:
              qum = u(t07, m[n]);
              mi[n] = (u(t07, (m[n] + 1)) - qum) / (float)tdjk;
              y[n] = (mi[n] * (float(td[n]) - (tdjk * p[n]))) + qum;
              break;

            case 1:
              qvm = v(t07, m[n]);
              mi[n] = (v(t07, (m[n] + 1)) - qvm) / (float)tdjk;
              y[n] = (mi[n] * (float(td[n]) - (tdjk * p[n]))) + qvm;
              break;

            case 2:
              qwm = w(t07, m[n]);
              mi[n] = (w(t07, (m[n] + 1)) - qwm) / (float)tdjk;
              y[n] = (mi[n] * (float(td[n]) - (tdjk * p[n]))) + qwm;
              break;

            case 3:
              qcm = g(t07, m[n]);
              mi[n] = (g(t07, (m[n] + 1)) - qcm) / (float)tdjk;
              y[n] = (mi[n] * (float(td[n]) - (tdjk * p[n]))) + qcm;
              break;

            default:
              break;
          }
        }

        if (t[n] > tdijp[n] + ti[n])
        {
          tdijp[n] += tdjk;
          p[n]++;
          m[n]++;
        }
      }
    }

    j11.write(y[0]); //run servos
    j2.write(y[1]);
    j3.write(y[2]);
    j6.write(y[3] + 20);
  }

  for (n = 0; n < 4; n++)
  {
    tjkk[n] = 0;
    tdijo[n] = tdjk;
    tdijp[n] = tdjk;
    tj[n] = 0;
    o[n] = 0;
    p[n] = 0;
  }
}

float u(float t06[][4][4], int n)
{
  float xc = t06[n][0][3] - (d6 * t06[n][0][2]);
  float yc = t06[n][1][3] - (d6 * t06[n][1][2]);
  float zc = t06[n][2][3] - (d6 * t06[n][2][2]);

  float q1 = atan(yc / xc);

  if (xc > 1 && yc > 1)
  {
    q1 = (q1 * 180 / M_PI);
  }
  if (xc < 1 && yc > 1)
  {
    q1 = (q1 * 180 / M_PI) + 180;
  }
  if (xc == 0 && yc > 1)
  {
    q1 = 90;
  }

  return (q1);
}

float v(float t06[][4][4], int n)
{
  float xc = t06[n][0][3] - (d6 * t06[n][0][2]);
  float yc = t06[n][1][3] - (d6 * t06[n][1][2]);
  float zc = t06[n][2][3] - (d6 * t06[n][2][2]);

  float r = sqrt((xc * xc) + (yc * yc)) - l0; //r=sqrt((pow(xc,2))+pow(yc,2))-l0;
  float pwz = zc - d1;
  float s = sqrt((r * r) + (pwz * pwz)); //s=sqrt((pow(r,2))+pow(pwz,2));
  float a = atan(pwz / r);
  float b = fabs(acos(((s * s) + (l2 * l2) - (d4 * d4)) / (2 * l2 * s))); //b=fabs(acos((pow(s,2)+pow(l2,2)-pow(d4,2))/(2*l2*s)));
  float q2 = 1.5708 - a - b; //q2=(M_PI/2)-a-b;
  q2 = (q2 * 57.2958) + 90; //q2=(q2*180/M_PI)+90;

  return (q2);
}

float w(float t06[][4][4], int n)
{
  float xc = t06[n][0][3] - (d6 * t06[n][0][2]);
  float yc = t06[n][1][3] - (d6 * t06[n][1][2]);
  float zc = t06[n][2][3] - (d6 * t06[n][2][2]);

  float r = sqrt((xc * xc) + (yc * yc)) - l0; //r=sqrt((pow(xc,2))+pow(yc,2))-l0;
  float pwz = zc - d1;
  float s = sqrt((r * r) + (pwz * pwz)); //s=sqrt((pow(r,2))+pow(pwz,2));
  float g = fabs(acos(((l2 * l2 * 1.0) + (d4 * d4) - (s * s)) / (2 * l2 * (float)(d4)))); //2*l2*d4=36000, g=fabs(acos((pow(l2,2)+pow(d4,2)-pow(s,2))/(36000))) !
  float q3 = (1.5708) - g; //q3=(M_PI/2)-g;
  q3 = ((-1) * q3 * 57.2958) + 90;

  return (q3);
}

float g(float t06[][4][4], int n)
{
  float xc = t06[n][0][3] - (d6 * t06[n][0][2]);
  float yc = t06[n][1][3] - (d6 * t06[n][1][2]);
  float zc = t06[n][2][3] - (d6 * t06[n][2][2]);

  float r = sqrt((xc * xc) + (yc * yc)) - l0; //r=sqrt((pow(xc,2))+pow(yc,2))-l0;
  float pwz = zc - d1;
  float s = sqrt((r * r) + (pwz * pwz)); //s=sqrt((pow(r,2))+pow(pwz,2));
  float a = atan(pwz / r);
  float b = fabs(acos(((s * s) + (l2 * l2) - (d4 * d4)) / (2 * l2 * s))); //b=fabs(acos((pow(s,2)+pow(l2,2)-pow(d4,2))/(2*l2*s)));
  float g = fabs(acos(((l2 * l2 * 1.0) + (d4 * d4) - (s * s)) / (2 * l2 * (float)(d4)))); //g=fabs(acos((pow(l2,2)+pow(d4,2)-pow(s,2))/(36300)));//2*l2*d4=36000
  float p = 180 - (57.2957 * (b + g)); //p=180-(180*(b+g)/M_PI);
  q6 = p - (a * 57.2957) + 90; //q4=p-(a*180/M_PI);

  return (q6);
}

double sgn(double x)
{
  if (x > 0.0) return 1.0;
  if (x < 0.0) return -1.0;
  if (x == 0.0) return 0;
  return x;
}

Not sure the question. i will guess that you would like to slow the servos down. There are libraries to help with that.

The MobaTools library has a class for servos that can control servo speed.

The ServoEasing library allows servo speed control as well as tailored motion.

Yes.

My solution was to comment my code so it was readable AND understandable by any programmer.

No the problem occures only when starting on the microcontrollers. The servo motors starts turning very fast randomly after that they can be controllable...

No, The problem occurs only when starting the microcontroller, the motors keep turning randomly very fast for like one or two seconds after that they can be controllable

Turn off your Serial Monitor or disconnect the USB cable and use the Arduino "power" or "reset" button to restart the sketch. I have an Arduino that will reset if I open a serial monitor after the sketch is running.

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.