Prueba 3 por fin funciona!!

Código 3
#include <Servo.h>
#define Juallekcreator
//servos
Servo servo1;//base
Servo servo2;//codo
Servo servo3;//mano
Servo servo4;//pinza
//arreglos
int paso1[20];
int paso2[20];
int paso3[20];
int paso4[20];
//enteros
int j=0,i=0,h=0,r,pasomax=0;
//tiempo
long tiempoc=0;
unsigned long tiempot=millis();
//ejecutar
boolean automata=false, primer_memoria=true, primer_paso=true;

void setup()
{
  pinMode(6, INPUT);
  pinMode(7, INPUT);
  pinMode(13, OUTPUT);
  servo1.attach(8);
  servo2.attach(9);
  servo3.attach(10);
  servo4.attach(11);
  Serial.begin(115200);
  delay(2500);
  Serial.print("Robot inicio espere...");
  digitalWrite(13,HIGH);
  delay(1000);
  digitalWrite(13,LOW);
}

void loop()
{
  tiempot=millis();
  boton();
  if(!automata)
  {
  mover();
  }
  else if(automata)
  {
    if(primer_paso==true)
    {
      j=0;
    }
    else if(j>pasomax)
    {
      j=0;
    }
    ejecutar_automata();
  }
  h++;
  Serial.print("\tcompilacion_#"); Serial.print(h);
}
void boton()
{
  if((digitalRead(6))==false)
  {
    delay(20);
    if((digitalRead(6))==true)
    {
      if(i==0)
      {
        i=1;
        tiempoc=tiempot;
      }
      else if((i==1)&&((tiempot-tiempoc)<250))
      {
        i=2;
      }
    }
  }  

  if((i==1)&&((tiempot-tiempoc)>1000))
  {
    if(primer_memoria==true)
    {
      j=0;
    }
    memoria();
    automata=false;
    digitalWrite(13,HIGH);
    delay(100);
    digitalWrite(13,LOW);
    pasomax=j;
    primer_memoria=false;
    i=0;
    j++;
  }
  else if(i==2)
  {
    automata=true;
    Serial.print("\n****************************************ejecutar_automata****************************************");
    i=0;
  }
}
void memoria()
{
  Serial.print("\n------------------------------------------------------------memoria"); Serial.print(j); Serial.print("_lista------------------------------------------------------------");
  paso1[j] = map(analogRead(A0), 157, 873, 0, 180);
  paso2[j] = map(analogRead(A1), 157, 873, 0, 180);
  paso3[j] = map(analogRead(A2), 157, 873, 0, 180);
  paso4[j] = map(analogRead(A3), 157, 873, 0, 180);

}
void ejecutar_automata()
{
    servo1.write(paso1[j]);
    servo2.write(paso2[j]);
    servo3.write(paso3[j]);
    servo4.write(paso4[j]);
    Serial.print("\nejecutando_memoria"); Serial.print(j);
    delay(800);
    j++;
    while (digitalRead(7)==true)
    {
      Serial.print("\n\tautomata_pause");
      digitalWrite(13, HIGH); delay(500);
      digitalWrite(13, LOW); delay(500);
    }
    primer_paso=false;
}
void mover()
{
  Serial.print("\nmemoria_en_espera...");
  int paso1=map(analogRead(A0),157,873,0,180);
  int paso2=map(analogRead(A1),157,873,0,180);
  int paso3=map(analogRead(A2),157,873,0,180);
  int paso4=map(analogRead(A3),157,873,0,180);
  servo1.write(paso1);
  servo2.write(paso2);
  servo3.write(paso3);
  servo4.write(paso4);

}

código 4

-control de pinza con pulsador

-control de velocidad de desplazamiento de distancias regulado por potencio metro

   Memoria-- modificado hasta el código 5

   guarda posiciones y las define como limites

   calcula cual es la menor y dividir todas las distancias para hacer un rango de desplazamiento

   asigna en un grupo de arreglos definido por la distancia menor los rangos

   

   Autómata-- modificado hasta el código 5

   mueve desde los arreglos de rangos, y después de posiciones de cada memoria

   asigna un delay dependiendo de una lectura analógica

   

#include <Servo.h>
#define Juallekcreator

//servos

Servo servo1;//base

Servo servo2;//codo

Servo servo3;//mano

Servo servo4;//pinza

//arreglos

int paso1[20];

int paso2[20];  

int paso3[20];  

int paso4[20];

//enteros

int j=0,i=0,h=0,r,pasomax=0;

//tiempo

long tiempoc=0;

unsigned long tiempot=millis();

//ejecutar

boolean automata=false, primer_memoria=true, primer_paso=true;

//control de pinza

int pasopinza=0,estant=0;

boolean pinza_cerrada=false;

void setup() 

{

  pinMode(5, INPUT);

  pinMode(6, INPUT);  

  pinMode(7, INPUT);  

  pinMode(13, OUTPUT);

  servo1.attach(8);

  servo2.attach(9);

  servo3.attach(10);

  servo4.attach(11);

  Serial.begin(115200);

  delay(2500);

  Serial.print("Robot inicio espere...");

  digitalWrite(13,HIGH);

  delay(1000);

  digitalWrite(13,LOW);

}

void loop() 

{

  tiempot=millis();

  boton();

  if(!automata)

  {

  mover();

  }

  else if(automata)

  {

    if(primer_paso==true)

    {

      j=0;

    }

    else if(j>pasomax)

    {

      j=0;

    }

    ejecutar_automata();

  }

  h++;

  Serial.print("\tcompilacion_#"); Serial.print(h);

}

void boton()

{

  if((digitalRead(6))==false)

  {

    delay(20);

    if((digitalRead(6))==true)

    {

      if(i==0)

      {

        i=1;

        tiempoc=tiempot;

      }

      else if((i==1)&&((tiempot-tiempoc)<250))

      {

        i=2;

      }

    }

  }     

  

  if((i==1)&&((tiempot-tiempoc)>1000))

  {

    if(primer_memoria==true)

    {

      j=0;

    }

    memoria();

    automata=false;

    digitalWrite(13,HIGH);

    delay(100);

    digitalWrite(13,LOW);

    pasomax=j;

    primer_memoria=false;

    i=0;

    j++;

  }

  else if(i==2)

  {

    automata=true;

    Serial.print("\n****************************************ejecutar_automata****************************************");

    i=0;

  }

}

void memoria()

{

  Serial.print("\n------------------------------------------------------------memoria"); Serial.print(j);   Serial.print("_lista------------------------------------------------------------");

  paso1[j] = map(analogRead(A0), 157, 873, 0, 180);

  paso2[j] = map(analogRead(A1), 157, 873, 0, 180);  

  paso3[j] = map(analogRead(A2), 157, 873, 0, 180);

  comparacion_pinza(); 

  paso4[j] = pasopinza;

  

}

void ejecutar_automata()

{

    servo1.write(paso1[j]);

    servo2.write(paso2[j]);

    servo3.write(paso3[j]);

    servo4.write(paso4[j]);

    Serial.print("\nejecutando_memoria"); Serial.print(j);

    delay(800);

    j++;

    while (digitalRead(7)==true)

    { 

      Serial.print("\n\tautomata_pause");

      digitalWrite(13, HIGH); delay(500);   

      digitalWrite(13, LOW); delay(500);

    }

    primer_paso=false;

}

void mover()

{

  Serial.print("\nmemoria_en_espera...");

  int paso1=map(analogRead(A0),157,873,0,180);

  int paso2=map(analogRead(A1),157,873,0,180);

  int paso3=map(analogRead(A2),157,873,0,180);

  boton_pinza();

  comparacion_pinza();

  servo1.write(paso1);

  servo2.write(paso2);

  servo3.write(paso3);

  servo4.write(pasopinza);

}

//seccion de movimiento de pinza

void boton_pinza()

{

  if((digitalRead(5))==false&&(estant==0))

  {

    delay(20);

    if((digitalRead(5))==true)

    {

      estant=1;

      pinza_cerrada=true;

    }

  }

  else if((digitalRead(5))==false&&(estant==1))

  {

    delay(20);

    if((digitalRead(5))==true)

    {

      estant=0;

      pinza_cerrada=false;

    }

  }

}

//esta sección complementa la sección de movimiento de pinza

void comparacion_pinza()

{

  if(pinza_cerrada==false)

  {

    pasopinza=180;

  }

  else if(pinza_cerrada==true)

  {

    pasopinza=0;

  }

}

//esta sección complementa la sección de memoria lista en código 5

void distancia_min()

{

  

}

void ajuste_pasos()

{

  

}