Prueba 5

Memorias en secciones

Se modifico el programa para optimizar la memoria, mejorar el movimiento, filtrar temblor de mano y mejorar la precisión al tomar objetos.

Por accidente corregí unos acentos, si te marca errores solo con borrarlos basta.

Código 5
#include <Servo.h>
#define Juallekcreator
Servo servo1;//base
Servo servo2;//codo
Servo servo3;//mano
Servo servo4;;//muñeca
Servo servo5;//pinza
//arreglos, solo pueden memorizar 19 posiciones con esta configuración
int memoria1[20];
int memoria2[20];
int memoria3[20];
int memoria4[20];
int memoria5[20];
//arreglos para cálculos
double paso[5],incremento[5],diferencia[5],registro[5],direccion[5];
//contadores
int pasoN=0,i=0,pasomax=0,seccion=1,limite=0;
//tiempo
long tiempoc=0;
long tiempob=0;
unsigned long tiempotranscurrido=millis();
unsigned long tiempomicros=micros();
int time=1000;
//constantes
#define retardo 4000
//booleanos
boolean automata=false, primer_memoria=true, primer_paso=true, pinzaabierta=true, primer_seccion=true, ultima_memoria=true;

void setup()
{
  pinMode(5,INPUT);//swith de pinza
  pinMode(6, INPUT);//memoria  
  pinMode(7, INPUT);//sistema en pausa  
  pinMode(13, OUTPUT);
  servo1.attach(8);
  servo2.attach(9);
  servo3.attach(10);
  servo4.attach(11);
  servo5.attach(12);
  Serial.begin(115200);
}

void loop()
{
  tiempotranscurrido=millis();
  tiempomicros=micros();
  boton();
  if(!automata)
  {
  mover();
  }
  else if(automata)
  {
    if(primer_paso==true)
    {
      if(ultima_memoria)ultimo_registro();
      pasoN=0;
      primer_paso=false;
    }
    else if(pasoN>pasomax)
    {
      pasoN=0;
    }
    if(primer_sección)matriz_de_traslación();
    if(tiempomicros-tiempob>time)
    {
      tiempob=tiempomicros;
      ejecutar_automata();
    }
  }
}

void boton()
{
  if((digitalRead(6))==LOW)
  {
    delay(20);
    if((digitalRead(6))==HIGH)
    {
      if(i==0)
      {
        i=1;
        tiempoc=tiempotranscurrido;
      }
      else if((i==1)&&((tiempotranscurrido-tiempoc)<250))
      {
        i=2;
      }
    }
  }  
  if((i==1)&&((tiempotranscurrido-tiempoc)>1000))
  {
    pasoN++;
    if(primer_memoria==true)pasoN=0;
    memoria();
    digitalWrite(13,HIGH);
    delay(100);
    digitalWrite(13,LOW);
    pasomax=pasoN;
    primer_memoria=false;
    i=0;
  }
  else if(i==2)
  {
    automata=true;
    Serial.print("\n****************************************EJECUTAR_AUTOMATA****************************************");
    i=0;
  }
}

void mover()
{
  lectura();
  servo1.write(paso[0]);
  servo2.write(paso[1]);
  servo3.write(paso[2]);
  servo4.write(paso[3]);
  servo5.write(paso[4]);
}

void ejecutar_automata()
{
  if(seccion<=limite)
  {
    Serial.print("\n\t\t\t\t\t\t\t\tEJECUTANDO_MEMORIA ");Serial.print(pasoN);
    Serial.print("\n\t\t\t\t\t\t\t\tSECCION="); Serial.print(seccion);
    seccion++;
    if(seccion == 1) time = retardo*4;      
    else if(seccion == 10) time = retardo*3;  
    else if(seccion == 20) time = retardo*2;
    else if(seccion == 30) time = retardo*1;
    if(seccion == limite-40) time = retardo*2;
    else if(seccion == limite-30) time = retardo*3;
    else if(seccion == limite-20) time = retardo*4;
    else if(seccion == limite-10) time = retardo*5;
    registro[0]+=direccion[0];
    registro[1]+=direccion[1];
    registro[2]+=direccion[2];
    registro[3]+=direccion[3];
    registro[4]+=direccion[4];
    //habilitar para mover en autómata
    servo1.write(registro[0]);
    servo2.write(registro[1]);
    servo3.write(registro[2]);
    servo4.write(registro[3]);
    servo5.write(registro[4]);
    primer_seccion=false;
  }
  else
  {
    limite=0;
    sección=0;
    pasoN++;
    primer_sección=true;
  }
  while (digitalRead(7)==true)
  {
    servo1.write(90);
    servo2.write(10);//rango inclinado para ganar distancia guardar a 10
    servo3.write(150);//rango inclinado para ganar distancia guardar a 150
    servo4.write(90);
    servo5.write(90);//rango de pinza 90-130
    digitalWrite(13, HIGH); delay(500);
    digitalWrite(13, LOW); delay(500);
  }
}


void matriz_de_traslacion()
{
  Serial.print("\n\t\t\t\t\t\t\t\tCALCULO DE MATRIZ");
  //calcular diferencias
  diferencia[0] = abs (registro[0]-memoria1[pasoN]);
  diferencia[1] = abs (registro[1]-memoria2[pasoN]);
  diferencia[2] = abs (registro[2]-memoria3[pasoN]);
  diferencia[3] = abs (registro[3]-memoria4[pasoN]);
  diferencia[4] = abs (registro[4]-memoria5[pasoN]);
   //filtrar vibraciones 
  if(diferencia[0]<=5)diferencia[0]=0;
  if(diferencia[1]<=5)diferencia[1]=0;
  if(diferencia[2]<=5)diferencia[2]=0;
  if(diferencia[3]<=5)diferencia[3]=0;
  if(diferencia[4]<=5)diferencia[4]=0;
  //calcular limite de secciones
  limite = max(diferencia[0],diferencia[1]);
  limite = max(limite,diferencia[2]);
  limite = max(limite,diferencia[3]);
  limite = max(limite,diferencia[4]);
  Serial.print("\n\t\t\t\t\t\t\t\tLIMITE=");Serial.print(limite);
  //calcula dirección e incremento en grados de cada sección
  if(registro[0]>memoria1[pasoN]) direccion[0]=(0-diferencia[0])/limite; else direccion[0]=diferencia[0]/limite;
  if(registro[1]>memoria2[pasoN]) direccion[1]=(0-diferencia[1])/limite; else direccion[1]=diferencia[1]/limite;
  if(registro[2]>memoria3[pasoN]) direccion[2]=(0-diferencia[2])/limite; else direccion[2]=diferencia[2]/limite;
  if(registro[3]>memoria4[pasoN]) direccion[3]=(0-diferencia[3])/limite; else direccion[3]=diferencia[3]/limite;
  if(registro[4]>memoria5[pasoN]) direccion[4]=(0-diferencia[4])/limite; else direccion[4]=diferencia[4]/limite;
}


void ultimo_registro()
{
  registro[0] = memoria1[pasoN];
  registro[1] = memoria2[pasoN];
  registro[2] = memoria3[pasoN];
  registro[3] = memoria4[pasoN];
  registro[4] = memoria5[pasoN];
  ultima_memoria=false;
}

void memoria()
{
  Serial.print("\n\t\t\t\t\t\tMEMORIA\t\t"); Serial.print(pasoN);
  lectura();
  memoria1[pasoN] = paso[0];
  memoria2[pasoN] = paso[1];
  memoria3[pasoN] = paso[2];
  memoria4[pasoN] = paso[3];
  memoria5[pasoN] = paso[4];
}
void lectura()
{
  paso[0]=map(analogRead(A0),157,873,0,180);
  paso[1]=map(analogRead(A1),157,873,0,180);
  paso[2]=map(analogRead(A2),157,873,0,180);
  paso[3]=map(analogRead(A3),157,873,0,180);
  if((digitalRead(5))==HIGH)paso[4]=90;else paso[4]=130;
}

Diagrama de conexión 
El nuevo código utiliza una pinza controlada por un switch, un servo adicional y un potencio metro extra. los potencio metros usados en el proyecto son de 20k pero lo ideal es 22k.