MacBot-1 (my first robotic arm)

I think this is a great project to grind through. Looks like the mechanical part is not difficult for you but the coding is.

I have a few questions for you:

• Did you try to build a simpler arm as a warm-up? Just wondering.
• Why do you say:
  

  Posted by: @macoofer

  the arduino has single processing serial output

  Aren't you controlling the servos directly using PWM pins, not Serial?

• Are you familiar with using millis() and "tight loops" for timing, as in the built-in sketch BlinkWithoutDelay? This tutorial or similar may help: https://www.programmingelectronics.com/arduino-millis-multiple-things/

@macoofer

Looking good!

Arduino can kind of perform some parallel writes by manipulating port registers directly.

There is a good reference here: Arduino Port Manipulation

Cheers!

@bushgeek

Thanks for the suggestions!

Before making this actual robot arm, I made a test setup with just two servos on a breadboard (I still have this setup as a means to quickly try out new code.)

During my searches I came across the millis(), looked into that and found a bunch of examples on using that with servos but in a very unclear way to me. With a very useful link from @robotbuilder, posted in another topic, I managed to get a better grasp of that concept.

I hadn't been able to get back into it due to work, but will very soon.

The "the arduino has single processing serial output" may have been confusing in the way I wrote it.

What I meant by it is that the arduino cannot straight up run parallel tasks and goes through code sequentially (if, for and while loops). In order to break this down, I have to use the suggested millis() or similar.

So yah, the mechanical part is not so much my limitation, my newbee knowledge on Arduino's (or any microcontroller) and the programming of them, since I literally just started to learn the code, is my main challenge. 😀 

But, as I was hoping when I joined this forum. There are quite a few people in here (you included 😉 ) that have skills in that, that are far beyond mine and have shown to be willing to help. And already did!

So, I'm going to dive deeper into the millis() section and experiment with that this weekend.

I'll post my findings here 😊 

@frogandtoad

Thanks for the link,

However, in my current stage of knowlegde I'm not sure if that is the kind of tweaking-depth I should dive in atm, lol

I got some good links to the millis() function, I just need to fiddle around with that and write some additional dissecting code to calculate the various distances and speeds for each movement on each servo. I know it can be done, since I've seen people use it, now I just have to understand it and do it myself 😉 

@macoofer

Posted by: @macoofer

Thanks for the link,

However, in my current stage of knowlegde I'm not sure if that is the kind of tweaking-depth I should dive in atm, lol

Not a problem, I thought you had a grip on it as you mentioned wanting to emulate multitasking... something to look into when you become more comfortable with Arduino.

Cheers!

Well, after some electronics-downtime, due to work and such.

And some time to read up on some more coding I conjured up a new piece of code to see if I could get this to work, with the millis()  funtion.

And the good news is...   IT WORKS!

Behold!:

So, it is still a test setup, driving 4 servos atm,  but I finally got them to do what i wanted.

For the synchronised movement of all degrees of freedom on my robotarm it can now independantly move all of them, adjusting their speeds automatically so they all start and end at the same time, regardsless of how far they have to move and in which direction.

To be continued...

Macoofer

p.s. Thanks again to those who pointed me out in the direction of those very usefull millis() tutorials!! 

Update:

I managed to make  a new enhanced version that now reads 8 different variable positions sequentially from an array. 

The next step is to clean up the code and put the rest of it in functions, then make the array of positions programmable with the existing setup I already had, using pot-meters and store-in-mem and play buttons.

This is how they move now:

This is the Serial monitor output that runs along side for debugging purposes

Below: The full code so far, its far from done yet, but if you are trying to get similar things done, feel free to use.

#include <Servo.h>
#include <Wire.h>
#include <hd44780.h>
#include <hd44780ioClass/hd44780_I2Cexp.h>

const long int MaxServos = 4; // Define the total amount of servos
const int ArraySteps = 8; // Defines the number of memory steps in the PosSave Arrays
int Servo1Done = 0; // Checker for Servo1-4 to see if its movement is completed
int Servo2Done = 0;
int Servo3Done = 0;
int Servo4Done = 0;
int ServoDoneCounter = MaxServos+1; // Initialize servomovement checker (if all servos are done moving for the current sequence)
const long int StepSpeed =15; // Define base servo movement speed factor (smaller is faster)
long int i,j,temp; // Define variable for sorting funtion
long int AscArray[MaxServos]; // Define ascending order sorting array
int show =1; // Test parameter
int RunServos=1; // Test parameter

//define the servos
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

//define array of "memory stored" positions for each servo (in below case: 8 steps)
//positions are in degrees and will be mapped into microseconds movement
long int Servo1PosSave[]={260,65,114,80,30,90,200,135};
long int Servo2PosSave[]={100,100,114,45,135,210,150,135};
long int Servo3PosSave[]={45,175,114,270,170,200,145,135};
long int Servo4PosSave[]={15,35,65,115,45,240,180,135};
int PosStep = 0; //The Global counter for the position of the servo pos save array

long int S1MP; //Servo1MicrosecondsPosition ( to move to )
long int S2MP;
long int S3MP;
long int S4MP;

// define variables for servo step movement (newpos)
long int NewPos1;
long int NewPos2;
long int NewPos3;
long int NewPos4;

//define array for variables of current servo positions
long int ServoPosPrevious[MaxServos];

//define array for distance to move of each servo, can be negative!
long int dtm[MaxServos];

//define array for absolute(positive values) distance, in order to calculate max distancefor each servo, must be postive!
long int adtm[MaxServos];

//define times variables
unsigned long CurrentTime;
unsigned long PreviousTime1=0;
unsigned long PreviousTime2=0;
unsigned long PreviousTime3=0;
unsigned long PreviousTime4=0;
unsigned long Longest_Time;
unsigned long Servo1_Time;
unsigned long Servo2_Time;
unsigned long Servo3_Time;
unsigned long Servo4_Time;

void setup() {
// put your setup code here, to run once:
Serial.begin(115200);

//define attached pins of the servos
servo1.attach(3);
servo2.attach(4);
servo3.attach(5);
servo4.attach(6);

//Set servos to test startposition of 90 degrees (midpoint of 500 ~ 2500 range =1500)
servo1.writeMicroseconds(1500);
servo2.writeMicroseconds(1500);
servo3.writeMicroseconds(1500);
servo4.writeMicroseconds(1500);
delay(2000); //Wait for 2 seconds before starting the loop

}

void loop() 
{
//Keep updating internal clock counter
CurrentTime=millis();
// Check is servo movement is due
if(ServoDoneCounter==5 && PosStep<ArraySteps)
{
// Calcualte all the math for the next servo movements
MapNextArrayPositions();
GetLastPositions();
DoSequenceMath();
}

//Move all the Servos
if (RunServos=1) //Check if any movement is needed for servos
{
if(ServoDoneCounter<MaxServos)
{
MoveAllServos();
}
if(ServoDoneCounter==MaxServos)
{
Serial.println("Servo movement for current array-step done...");
delay(1000); //Wait 1 second before going to next step
ServoDoneCounter=5; //Set ServoDoneCounter to sub-idle state
Servo1Done=0;
Servo2Done=0;
Servo3Done=0;
Servo4Done=0;
}
if(PosStep==ArraySteps && ServoDoneCounter==5)
{
Serial.println(F("All steps from memory array performed!...."));
ServoDoneCounter=6; //Set ServoDoneCounter to full-idle state
}
} 
}


// Delaration of functions below

void MoveServo1()
{
if (Servo1_Time>0)
{
if ((CurrentTime - PreviousTime1 >= Servo1_Time/5) && (NewPos1 != S1MP)) 
{ 
if(dtm[0]<0)
{
NewPos1 = NewPos1 -1;
servo1.writeMicroseconds(NewPos1);
}
else
{
NewPos1 = NewPos1 +1;
servo1.writeMicroseconds(NewPos1);
}
PreviousTime1 = CurrentTime;
if(NewPos1 == S1MP)
{
Servo1Done = 1;
}
} 
}
else
{
Servo1Done = 1;
}
}

void MoveServo2()
{
if (Servo2_Time>0)
{
if ((CurrentTime - PreviousTime2 >= Servo2_Time/5) && (NewPos2 != S2MP)) 
{ 
if(dtm[1]<0)
{
NewPos2 = NewPos2 -1;
servo2.writeMicroseconds(NewPos2);
}
else
{
NewPos2 = NewPos2 +1;
servo2.writeMicroseconds(NewPos2);
}
PreviousTime2 = CurrentTime;
if(NewPos2 == S2MP)
{
Servo2Done =1;
}
} 
}
else
{
Servo2Done = 1;
}
}

void MoveServo3()
{
if (Servo3_Time>0)
{
if ((CurrentTime - PreviousTime3 >= Servo3_Time/5) && (NewPos3 != S3MP)) 
{ 
if(dtm[2]<0)
{
NewPos3 = NewPos3 -1;
servo3.writeMicroseconds(NewPos3);
}
else
{
NewPos3 = NewPos3 +1;
servo3.writeMicroseconds(NewPos3);
}
PreviousTime3 = CurrentTime;
if(NewPos3 == S3MP)
{
Servo3Done =1;
}
} 
}
else
{
Servo3Done = 1;
}
}

void MoveServo4()
{
if (Servo4_Time>0)
{
if ((CurrentTime - PreviousTime4 >= Servo4_Time/5) && (NewPos4 != S4MP)) 
{ 
if(dtm[3]<0)
{
NewPos4 = NewPos4 -1;
servo4.writeMicroseconds(NewPos4);
}
else
{
NewPos4 = NewPos4 +1;
servo4.writeMicroseconds(NewPos4);
}
PreviousTime4 = CurrentTime;
if(NewPos4 == S4MP)
{
Servo4Done =1;
}
} 
}
else
{
Servo4Done = 1;
}
}

void MoveAllServos()
{
MoveServo1();
MoveServo2();
MoveServo3();
MoveServo4();
ServoDoneCounter = Servo1Done+Servo2Done+Servo3Done+Servo4Done; //Add up individual servo states to see if sequence is done
}

void MapNextArrayPositions()
{
//Map postitions from from degrees to microseconds from the holding array
S1MP = map(Servo1PosSave[PosStep],0,270,500,2500);
S2MP = map(Servo2PosSave[PosStep],0,270,500,2500);
S3MP = map(Servo3PosSave[PosStep],0,270,500,2500);
S4MP = map(Servo4PosSave[PosStep],0,270,500,2500);
Serial.println(F("Mapped positions from 0-270 degrees --> 500 - 2500 microseconds..."));
Serial.println(S1MP);
Serial.println(S2MP);
Serial.println(S3MP);
Serial.println(S4MP);
}

void GetLastPositions()
{
//Get the last written servo positions
ServoPosPrevious[0]=servo1.readMicroseconds();
ServoPosPrevious[1]=servo2.readMicroseconds();
ServoPosPrevious[2]=servo3.readMicroseconds();
ServoPosPrevious[3]=servo4.readMicroseconds();
Serial.println(F("Last written servo positions in microseconds..."));
Serial.println(ServoPosPrevious[0]);
Serial.println(ServoPosPrevious[1]);
Serial.println(ServoPosPrevious[2]);
Serial.println(ServoPosPrevious[3]);
}

void DoSequenceMath()
{
//Get positive values of next angle for each servo, when done, change the ServoDoneCounter state to 0, to start the Servo Movements
dtm[0]=S1MP-ServoPosPrevious[0];
if (dtm[0]<0)
{
adtm[0]=-dtm[0];
}
else adtm[0]=dtm[0]; 
dtm[1]=S2MP-ServoPosPrevious[1];
if (dtm[1]<0)
{
adtm[1]=-dtm[1];
}
else adtm[1]=dtm[1];
dtm[2]=S3MP-ServoPosPrevious[2];
if (dtm[2]<0)
{
adtm[2]=-dtm[2];
}
else adtm[2]=dtm[2];
dtm[3]=S4MP-ServoPosPrevious[3];
if (dtm[3]<0)
{
adtm[3]=-dtm[3];
}
else adtm[3]=dtm[3];

Serial.println(F("Absolute(positive) distance in total microseconds to move per servo..."));
Serial.println(adtm[0]);
Serial.println(adtm[1]);
Serial.println(adtm[2]);
Serial.println(adtm[3]);

// Testarray

Serial.println(F("Filling test array..."));
AscArray[0] = adtm[0];
AscArray[1] = adtm[1];
AscArray[2] = adtm[2];
AscArray[3] = adtm[3];

Serial.println(F("Sorting array..."));

//sorting - ASCENDING ORDER
for(i=0; i<MaxServos; i++)
{ 
for(j=i+1; j<MaxServos; j++)
{
if(AscArray[i]>AscArray[j])
{
temp =AscArray[i];
AscArray[i]=AscArray[j];
AscArray[j]=temp;
}
}
}

// Determine highest number from sorted array
Serial.println(F("Highest number from array: "));
Serial.println(AscArray[MaxServos-1]);

//calculate longest and subtimes for each servo
Longest_Time=(AscArray[MaxServos-1]*StepSpeed);
if (adtm[0]>0) 
{
Servo1_Time=Longest_Time/adtm[0];
}
else Servo1_Time=0;
if (adtm[1]>0) 
{
Servo2_Time=Longest_Time/adtm[1];
}
else Servo2_Time=0;
if (adtm[2]>0) 
{
Servo3_Time=Longest_Time/adtm[2];
}
else Servo3_Time=0;
if (adtm[3]>0) 
{
Servo4_Time=Longest_Time/adtm[3];
}
else Servo4_Time=0;
Serial.print(F("Longest time: "));
Serial.print(Longest_Time);
Serial.println(" msec");
Serial.print(F("Servo1 time/step: "));
Serial.print(Servo1_Time);
Serial.println(" msec");
Serial.print(F("Servo2 time/step: "));
Serial.print(Servo2_Time);
Serial.println(" msec");
Serial.print(F("Servo3 time/step: "));
Serial.print(Servo3_Time);
Serial.println(" msec");
Serial.print(F("Servo4 time/step: "));
Serial.print(Servo4_Time);
Serial.println(" msec");

//Set newpos start postition for movement
NewPos1=ServoPosPrevious[0];
NewPos2=ServoPosPrevious[1];
NewPos3=ServoPosPrevious[2];
NewPos4=ServoPosPrevious[3];
//Now that the math is done, set the ServoDoneCounter back to 0-state, so the serve movement function starts up
// and increase the PosStep in the memory array
ServoDoneCounter = 0;
PosStep++; 
}

Here is a better attempt to put the code:

#include <Servo.h>
#include <Wire.h>
#include <hd44780.h>
#include <hd44780ioClass/hd44780_I2Cexp.h>

const long int MaxServos = 4;      // Define the total amount of servos
const int ArraySteps = 8;          // Defines the number of memory steps in the PosSave Arrays
int Servo1Done = 0;                // Checker for Servo1-4 to see if its movement is completed
int Servo2Done = 0;
int Servo3Done = 0;
int Servo4Done = 0;
int ServoDoneCounter = MaxServos+1;  // Initialize servomovement checker (if all servos are done moving for the current sequence)
const long int StepSpeed =15;        // Define base servo movement speed factor (smaller is faster)
long int i,j,temp;                   // Define variable for sorting funtion
long int AscArray[MaxServos];        // Define ascending order sorting array
int show =1;                         // Test parameter
int RunServos=1;                     // Test parameter

//define the servos
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

//define array of "memory stored" positions for each servo (in below case: 8 steps)
//positions are in degrees and will be mapped into microseconds movement
long int Servo1PosSave[]={260,65,114,80,30,90,200,135};
long int Servo2PosSave[]={100,100,114,45,135,210,150,135};
long int Servo3PosSave[]={45,175,114,270,170,200,145,135};
long int Servo4PosSave[]={15,35,65,115,45,240,180,135};
int PosStep = 0;  //The Global counter for the position of the servo pos save array

long int S1MP; //Servo1MicrosecondsPosition ( to move to )
long int S2MP;
long int S3MP;
long int S4MP;

// define variables for servo step movement (newpos)
long int NewPos1;
long int NewPos2;
long int NewPos3;
long int NewPos4;

//define array for variables of current servo positions
long int ServoPosPrevious[MaxServos];

//define array for distance to move of each servo, can be negative!
long int dtm[MaxServos];

//define array for absolute(positive values) distance, in order to calculate max distancefor each servo, must be postive!
long int adtm[MaxServos];

//define times variables
unsigned long CurrentTime;
unsigned long PreviousTime1=0;
unsigned long PreviousTime2=0;
unsigned long PreviousTime3=0;
unsigned long PreviousTime4=0;
unsigned long Longest_Time;
unsigned long Servo1_Time;
unsigned long Servo2_Time;
unsigned long Servo3_Time;
unsigned long Servo4_Time;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  
  //define attached pins of the servos
  servo1.attach(3);
  servo2.attach(4);
  servo3.attach(5);
  servo4.attach(6);

  //Set servos to test startposition of 90 degrees (midpoint of 500 ~ 2500 range =1500)
  servo1.writeMicroseconds(1500);
  servo2.writeMicroseconds(1500);
  servo3.writeMicroseconds(1500);
  servo4.writeMicroseconds(1500);
  delay(2000);  //Wait for 2 seconds before starting the loop

}

void loop() 
{
  //Keep updating internal clock counter
  CurrentTime=millis();
  // Check is servo movement is due
  if(ServoDoneCounter==5 && PosStep<ArraySteps)
  {
    //  Calcualte all the math for the next servo movements
    MapNextArrayPositions();
    GetLastPositions();
    DoSequenceMath();
  }
  
  //Move all the Servos
  if (RunServos=1)  //Check if any movement is needed for servos
  {
    if(ServoDoneCounter<MaxServos)
    {
      MoveAllServos();
    }
    if(ServoDoneCounter==MaxServos)
    {
      Serial.println("Servo movement for current array-step done...");
      delay(1000);  //Wait 1 second before going to next step
      ServoDoneCounter=5;  //Set ServoDoneCounter to sub-idle state
      Servo1Done=0;
      Servo2Done=0;
      Servo3Done=0;
      Servo4Done=0;
    }
  if(PosStep==ArraySteps && ServoDoneCounter==5)
  {
    Serial.println(F("All steps from memory array performed!...."));
    ServoDoneCounter=6;  //Set ServoDoneCounter to full-idle state
  }
  }    
}


// Delaration of functions below

void MoveServo1()
{
  if (Servo1_Time>0)
       {
      if ((CurrentTime - PreviousTime1 >= Servo1_Time/5) && (NewPos1 != S1MP)) 
       { 
        if(dtm[0]<0)
        {
          NewPos1 = NewPos1 -1;
          servo1.writeMicroseconds(NewPos1);
        }
        else
        {
          NewPos1 = NewPos1 +1;
          servo1.writeMicroseconds(NewPos1);
        }
        PreviousTime1 = CurrentTime;
        if(NewPos1 == S1MP)
          {
            Servo1Done = 1;
          }
       }     
      }
      else
      {
        Servo1Done = 1;
      }
}

void MoveServo2()
{
  if (Servo2_Time>0)
      {
      if ((CurrentTime - PreviousTime2 >= Servo2_Time/5) && (NewPos2 != S2MP)) 
       { 
        if(dtm[1]<0)
        {
          NewPos2 = NewPos2 -1;
          servo2.writeMicroseconds(NewPos2);
        }
        else
        {
          NewPos2 = NewPos2 +1;
          servo2.writeMicroseconds(NewPos2);
        }
        PreviousTime2 = CurrentTime;
        if(NewPos2 == S2MP)
          {
            Servo2Done =1;
          }
       }     
      }
      else
      {
        Servo2Done = 1;
      }
}

void MoveServo3()
{
  if (Servo3_Time>0)
      {
      if ((CurrentTime - PreviousTime3 >= Servo3_Time/5) && (NewPos3 != S3MP)) 
       { 
        if(dtm[2]<0)
        {
          NewPos3 = NewPos3 -1;
          servo3.writeMicroseconds(NewPos3);
        }
        else
        {
          NewPos3 = NewPos3 +1;
          servo3.writeMicroseconds(NewPos3);
        }
        PreviousTime3 = CurrentTime;
        if(NewPos3 == S3MP)
          {
            Servo3Done =1;
          }
       }     
      }
      else
      {
        Servo3Done = 1;
      }
}

void MoveServo4()
{
  if (Servo4_Time>0)
      {
      if ((CurrentTime - PreviousTime4 >= Servo4_Time/5) && (NewPos4 != S4MP)) 
       { 
        if(dtm[3]<0)
        {
          NewPos4 = NewPos4 -1;
          servo4.writeMicroseconds(NewPos4);
        }
        else
        {
          NewPos4 = NewPos4 +1;
          servo4.writeMicroseconds(NewPos4);
        }
        PreviousTime4 = CurrentTime;
        if(NewPos4 == S4MP)
          {
            Servo4Done =1;
          }
       }     
      }
      else
      {
        Servo4Done = 1;
      }
}

void MoveAllServos()
{
    MoveServo1();
    MoveServo2();
    MoveServo3();
    MoveServo4();
    ServoDoneCounter = Servo1Done+Servo2Done+Servo3Done+Servo4Done;  //Add up individual servo states to see if sequence is done
}

void MapNextArrayPositions()
{
  //Map postitions from from degrees to microseconds from the holding array
  S1MP = map(Servo1PosSave[PosStep],0,270,500,2500);
  S2MP = map(Servo2PosSave[PosStep],0,270,500,2500);
  S3MP = map(Servo3PosSave[PosStep],0,270,500,2500);
  S4MP = map(Servo4PosSave[PosStep],0,270,500,2500);
  Serial.println(F("Mapped positions from 0-270 degrees --> 500 - 2500 microseconds..."));
  Serial.println(S1MP);
  Serial.println(S2MP);
  Serial.println(S3MP);
  Serial.println(S4MP);
}

void GetLastPositions()
{
  //Get the last written servo positions
  ServoPosPrevious[0]=servo1.readMicroseconds();
  ServoPosPrevious[1]=servo2.readMicroseconds();
  ServoPosPrevious[2]=servo3.readMicroseconds();
  ServoPosPrevious[3]=servo4.readMicroseconds();
  Serial.println(F("Last written servo positions in microseconds..."));
  Serial.println(ServoPosPrevious[0]);
  Serial.println(ServoPosPrevious[1]);
  Serial.println(ServoPosPrevious[2]);
  Serial.println(ServoPosPrevious[3]);
}

void DoSequenceMath()
{
  //Get positive values of next angle for each servo, when done, change the ServoDoneCounter state to 0, to start the Servo Movements
  dtm[0]=S1MP-ServoPosPrevious[0];
  if (dtm[0]<0)
    {
      adtm[0]=-dtm[0];
    }
    else adtm[0]=dtm[0];  
  dtm[1]=S2MP-ServoPosPrevious[1];
  if (dtm[1]<0)
    {
      adtm[1]=-dtm[1];
    }
    else adtm[1]=dtm[1];
  dtm[2]=S3MP-ServoPosPrevious[2];
  if (dtm[2]<0)
    {
      adtm[2]=-dtm[2];
    }
    else adtm[2]=dtm[2];
  dtm[3]=S4MP-ServoPosPrevious[3];
  if (dtm[3]<0)
    {
      adtm[3]=-dtm[3];
    }
    else adtm[3]=dtm[3];

  Serial.println(F("Absolute(positive) distance in total microseconds to move per servo..."));
  Serial.println(adtm[0]);
  Serial.println(adtm[1]);
  Serial.println(adtm[2]);
  Serial.println(adtm[3]);

  // Testarray 

  Serial.println(F("Filling test array..."));
  AscArray[0] = adtm[0];
  AscArray[1] = adtm[1];
  AscArray[2] = adtm[2];
  AscArray[3] = adtm[3];

  Serial.println(F("Sorting array..."));

  //sorting - ASCENDING ORDER
  for(i=0; i<MaxServos; i++)
  {   
    for(j=i+1; j<MaxServos; j++)
    {
      if(AscArray[i]>AscArray[j])
      {
        temp  =AscArray[i];
        AscArray[i]=AscArray[j];
        AscArray[j]=temp;
      }
    }
  }
  
  // Determine highest number from sorted array
  Serial.println(F("Highest number from array: "));
  Serial.println(AscArray[MaxServos-1]);

//calculate longest and subtimes for each servo
Longest_Time=(AscArray[MaxServos-1]*StepSpeed);
if (adtm[0]>0) 
  {
    Servo1_Time=Longest_Time/adtm[0];
  }
  else Servo1_Time=0;
  if (adtm[1]>0) 
  {
    Servo2_Time=Longest_Time/adtm[1];
  }
  else Servo2_Time=0;
  if (adtm[2]>0) 
  {
    Servo3_Time=Longest_Time/adtm[2];
  }
  else Servo3_Time=0;
  if (adtm[3]>0) 
  {
    Servo4_Time=Longest_Time/adtm[3];
  }
  else Servo4_Time=0;
Serial.print(F("Longest time: "));
Serial.print(Longest_Time);
Serial.println(" msec");
Serial.print(F("Servo1 time/step: "));
Serial.print(Servo1_Time);
Serial.println(" msec");
Serial.print(F("Servo2 time/step: "));
Serial.print(Servo2_Time);
Serial.println(" msec");
Serial.print(F("Servo3 time/step: "));
Serial.print(Servo3_Time);
Serial.println(" msec");
Serial.print(F("Servo4 time/step: "));
Serial.print(Servo4_Time);
Serial.println(" msec");

//Set newpos start postition for movement
NewPos1=ServoPosPrevious[0];
NewPos2=ServoPosPrevious[1];
NewPos3=ServoPosPrevious[2];
NewPos4=ServoPosPrevious[3];
//Now that the math is done, set the ServoDoneCounter back to 0-state, so the serve movement function starts up
// and increase the PosStep in the memory array
ServoDoneCounter = 0;
PosStep++;  
}

@macoofer

You actually had time to redo the previous post.  You can edit a post within a one hour time limit.

The source code is much easier to read although I note the indentations (tabs) are not consistent and the lines after the last else statement are not indented at all.

Ideally for training an arm you would hold the hand and move the arm manually while the changing positions were recorded rather than using POTS.  You can use an identical arm with POTS instead of motors and move that about to record the changing positions and also to control the motorised arm.

@robotbuilder

I found out about the better way to put the code 3 hours after I posted the previous post, so the edit option was no longer available, hence the additional post 😉 

My code is still a bit messy, but as mentioned, the cleaning part is the next step, I was already happy I got it to work this far with my limited knowledge on coding 🤣 

As for the suggested pot-arm, I actually was looking into that, so you could say that was a good anticipated pointer into the direction I needed to go!  Thanks 😊 

Guess its back to waiting for my additional parts to arrive, before I can make the 2nd arm to program the first one.

Meanwhile I'll work on the code...

"There's a way to do it better - find it."  - Thomas Edison

@macoofer

Another thought I had was perhaps signals from pressure sensors or switches in the joints that would be activated in the actual arm when a force was applied by someone trying to physically move the gripper end. These signals could be used to turn the motors in the direction the user was applying pressure. Any changes in the angle of the joints could be recorded for playback.

I would like to build a robotic arm myself but one which would be guided by feedback.  So for example you place a block on the table and using vision to locate the block and its orientation the program would compute the path the arm has to take and the opening/closing of the gripper required to pick up the block. This involves inverse kinematics to compute the rotation required for each joints to achieve this outcome. Of course is you are going to use ROS I would assume inverse kinematic computations would come with it.