Memorias en secciones
VIDEO
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.