MacBot-1 (my first ...
 
Notifications
Clear all

MacBot-1 (my first robotic arm)  

  RSS

Macoofer
(@macoofer)
Eminent Member
Joined: 5 months ago
Posts: 28
2020-05-10 10:23 pm  

Hiya folks,

 

I figured I might as well post some pics and such on the first robot I'm trying to create from scratch.

I haven't gotten very far yet, I'm waiting for parts, more knowledge on those parts and particularly on howto program the Arduino accordingly, apart from the fact that I started this robot project about 4 weeks ago.

It's a 6-DOF DIY 3D printed robotic arm, that when fully standing upwards is about 46cm tall.

The design is a modified design I found on thingiverse.

This current model is powered by 3 15kg servo motors for the waist, shoulder and elbow and 3 SG90 mini servo motors for the wrist, hand and gripper.

I printed all the parts with an Ender 3D Pro and the code I have so far to test it is a collection of various example codes I analyzed and stuck together to in the attempt to get it to do what I hope it will do 😊 .

20200510 224649

- The first picture is the modified design, made in Autodesk Fusion 360

20200510 224707

- The second picture is the current build, where I have all the servos in place, yet 3 are actually connected atm for testing purposes

20200510 224717

- The third picture is the prototyping wiring: Its based off an Arduino mega and the servos directly managed with the digital outs of the mega (Im waiting for some servo motor drivers to ship in) The current setup has a couple of pot-meters which I can use to move the arm in various positions. On the bottom right of the Breadboard you can see two small pushbuttons; when the left one is pushed it stores the current angles of the servos in an array ( this test setup has 8 memory positions). When the right button is pushed it goes through the 8 stored memory positions sequentially in a loop. 

20200510 224723

- The last picture is has two lcd displays, that spit out various real time data to help me understand the difference of what I think the robot is doing and what the robot is actually doing (I had a lot of errors and crashes due to my bad programming and couldn't explain why, adding these helped a lot)

The small 16x2 lcd displays the pot1 and pot2 values in both the analogue values (0-1023)  and the degrees they were mapped to (0-180)

The large 20x4 lcd displays the memory step it is currently at (n-8) The destination angle of a certain servo (in this case Servo2, the shoulder PD2) Po2 is the old, or last known sent angle to that servo.

and AgR2 (AngleRealtime)  shows the actual angle the servo is at.

(one of the things I noticed eg is that the actual angles slightly deviate from the planned destination angles)

I have gotten to the point where im able to smoothen the movement of the individual servo movements. but they are still all serial (waist->shoulder->elbow->etc) since the arduino has single processing serial output.

The next brainbreaker I'm currently trying to figure out is how to emulate multitasking, so that the arm can move all the servo's at the same time (smooth) with independent speeds, directions and angles. So that when I send a command to move eg. 3 servos in one "movement" or "memory-step" they all start at the same time and all finish at the same time, but due to the different amount of degrees they might all have to travel, one would have to move slower or faster than another.

 

Well, that's how far I've gotten so far!

I will keep you updated on the progress and will most likely knock on the forums' door for some help on tackling my current brainbreaker 


Quote
Topic Tags
bushgeek
(@bushgeek)
Active Member
Joined: 4 months ago
Posts: 6
2020-05-20 6:01 am  

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/

ReplyQuote
frogandtoad
(@frogandtoad)
Reputable Member
Joined: 1 year ago
Posts: 480
2020-05-20 6:24 am  

@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!


ReplyQuote
Macoofer
(@macoofer)
Eminent Member
Joined: 5 months ago
Posts: 28
2020-05-20 10:03 am  

@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 😊 


ReplyQuote
Macoofer
(@macoofer)
Eminent Member
Joined: 5 months ago
Posts: 28
2020-05-20 10:11 am  

@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 😉 


ReplyQuote
frogandtoad
(@frogandtoad)
Reputable Member
Joined: 1 year ago
Posts: 480
2020-05-21 12:04 pm  

@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!


Macoofer liked
ReplyQuote
Macoofer
(@macoofer)
Eminent Member
Joined: 5 months ago
Posts: 28
2020-06-08 4:00 pm  

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!! 


ReplyQuote
Macoofer
(@macoofer)
Eminent Member
Joined: 5 months ago
Posts: 28
2020-06-10 7:50 pm  

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++;
}

ReplyQuote
Macoofer
(@macoofer)
Eminent Member
Joined: 5 months ago
Posts: 28
2020-06-10 10:41 pm  

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++;  
}

 



ReplyQuote
robotBuilder
(@robotbuilder)
Reputable Member
Joined: 1 year ago
Posts: 493
2020-06-10 11:30 pm  

@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.

 

This post was modified 3 months ago by robotBuilder

ReplyQuote
Macoofer
(@macoofer)
Eminent Member
Joined: 5 months ago
Posts: 28
2020-06-11 12:31 am  

@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


ReplyQuote
robotBuilder
(@robotbuilder)
Reputable Member
Joined: 1 year ago
Posts: 493
2020-06-12 1:20 am  

@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.

 

This post was modified 3 months ago 4 times by robotBuilder

ReplyQuote