Prueba 6

Código 6
Básicamente utiliza el mismo diagrama que la prueba 5, solo que almacena las variables en la EEPROM. hace uso del control aun.

#include <Servo.h>
#include <EEPROM.h>
Servo servo1;//base
Servo servo2;//codo
Servo servo3;//mano
Servo servo4;//muñeca
Servo servo5;//pinza
/*arreglos solo pueden memorizar 20 posiciones con esta configuración (en el arduino UNO),
 el uso de la EEPROM limita la capacidad del brazo de aprender
 20 posiciones * 5 servos =memorias*/
byte memoria[105];
//arreglos para cálculos
byte memor[5];
double direccion[5], registro[5], diferencia[5];
//arreglo para movimiento sin memoria
byte paso[5];
//contadores
byte pasoN=0,i=0,seccion=1,limite=0;
//contadores EEPROM
byte eprom,EE=0;
//tiempo
long tiempoc=0;
long tiempob=0;
unsigned long tiempotranscurrido=millis();
unsigned long tiempomicros=micros();
byte 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()
{
  for(i=5;i<8;i++){
    pinMode(i,INPUT);
  }
  i=0;
  pinMode(13, OUTPUT);
  servo1.attach(8);
  servo2.attach(9);
  servo3.attach(10);
  servo4.attach(11);
  servo5.attach(12);
  Serial.begin(115200);
  Serial.print("\t\tIniciando | ");
  for(i=0;i<31;i++){
    Serial.print("###");
    delay(25);
  }
  i=0;
  Serial.print(" | 100% 0.75s");
  Serial.print("\n\n\t\tBIENVENIDO USUARIO :)\n");
  delay(500);
  Serial.print("\t\t''CODIGO 6'' ALMACENAMIENTO EN EEPROM-JUALLEK 2016");
}

void loop()
{
  tiempotranscurrido=millis();
  tiempomicros=micros();
  boton();
  if(!automata)
  {
  mover();
  }
  else if(automata)
  {
    if(primer_paso)
    {
      lectura();
      if(ultima_memoria)ultimo_registro();
      pasoN=0;
      eprom=1;//                                                       eprom (inicio de secuencias)
      primer_paso=false;
    }
    else if(pasoN>EEPROM.read(250))
    {
      pasoN=0;
      eprom=1;//                                                       eprom (reinicio al llegar a limite de memoria)
    }
    if(primer_seccion)matriz_de_traslacion();
    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)<500))
      {
        i=2;
      }
    }
  }  
  if((i==1)&&((tiempotranscurrido-tiempoc)>1000))
  {
    pasoN++;
    if(primer_memoria)
    {
      pasoN=0;
      eprom=1;//                                                       eprom (inicio de memorias)
    }
    memoriza();
    EEPROM.write(250,pasoN);
    primer_memoria=false;
    i=0;
  }
  else if(i==2)
  {
    automata=true;
    Serial.print("\n\t\t\t\t****************************************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];
    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;
    seccion=0;
    pasoN++;
    primer_seccion=true;
  }
  while (digitalRead(7)==true)
  {
    servo1.write(90);
    servo2.write(10);//rango inclinado para ganar distancia guardar a 10
    servo3.write(140);//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");
  //convierte la EEPROM en matriz
  for(EE=0;EE<5;EE++)
  {
    memor[EE]=EEPROM.read(eprom);
    eprom++;
  }
  EE=0;
  //calcular diferencias
  diferencia[0] = abs (registro[0]-memor[0]);
  diferencia[1] = abs (registro[1]-memor[1]);
  diferencia[2] = abs (registro[2]-memor[2]);
  diferencia[3] = abs (registro[3]-memor[3]);
  diferencia[4] = abs (registro[4]-memor[4]);
   //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]>memor[0]) direccion[0]=(0-diferencia[0])/limite; else
 direccion[0]=diferencia[0]/limite;
  if(registro[1]>memor[1]) direccion[1]=(0-diferencia[1])/limite; else
 direccion[1]=diferencia[1]/limite;
  if(registro[2]>memor[2]) direccion[2]=(0-diferencia[2])/limite; else
 direccion[2]=diferencia[2]/limite;
  if(registro[3]>memor[3]) direccion[3]=(0-diferencia[3])/limite; else
 direccion[3]=diferencia[3]/limite;
  if(registro[4]>memor[4]) direccion[4]=(0-diferencia[4])/limite; else direccion[4]=diferencia[4]/limite;
}


void ultimo_registro()
{
  registro[0] = paso[0];
  registro[1] = paso[1];
  registro[2] = paso[2];
  registro[3] = paso[3];
  registro[4] = paso[4];
  ultima_memoria=false;
}

void memoriza()
{
  lectura();
  //convierte la matriz posición en EEPROM
  for(EE=0;EE<5;EE++)
  {
    EEPROM.write(eprom,paso[EE]);
    eprom++;
  }
  EE=0;
  Serial.print("\n\t\t\t\t\t\tMEMORIA\t\t"); Serial.print(pasoN);
  digitalWrite(13,HIGH);
  delay(100);
  digitalWrite(13,LOW);
}
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;
  //sección que delimita la lectura de los potencio metros
  paso[0]=constrain(paso[0],0,180);
  paso[1]=constrain(paso[1],0,180);
  paso[2]=constrain(paso[2],0,180);
  paso[3]=constrain(paso[3],0,180);
  paso[4]=constrain(paso[4],0,180);
}