En general
Control:
- 4 potenciometros 20K ohms
- 2 pulsadores
- 1 switch
- 1 switch de limite
Brazo:
- 5 sg90
Durante todo el desarrollo del proyecto se han usado un "Arduino Leonardo" y un "Arduino UNO"
lunes, 29 de agosto de 2016
Prueba 5
Memorias en secciones
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.
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.
sábado, 30 de abril de 2016
Diagramas
Diagrama de conexión 1
Descripción: este es el diagrama de conexión que corresponde al código "codigo3" en el que se utilizan dos pines para pulsadores (uno para la función memoria y otro para la pausa) a diferencia de la "codigo4" que tiene en uso tres pulsadores (uno para la función memoria otro para pausa y otro para la pinza)
Material:
4 potencio metros.
4 servomotores.
1 fuente de 5v.
2 pulsadores.
2 resistencias de 330.
1 arduino UNO (solo en el diagrama... en el proyecto yo utilice un arduino leonardo).
1 protoboard.
Cables.
Diagrama de conexión 2
Descripción: este es el diagrama de conexión que corresponde al código "codigo4" en el que se utilizan tres pulsadores (uno para la función memoria otro para pausa y otro para la pinza)
Material:
3 potencio metros.
4 servomotores.
1 fuente de 5v.
3 pulsadores.
3 resistencias de 330.
1 arduino UNO (solo en el diagrama... en el proyecto yo utilice un arduino leonardo).
1 protoboard.
Cables.
Diagrama de flujo
Descripción: este es el diagrama de conexión que corresponde al código "codigo3" en el que se utilizan dos pines para pulsadores (uno para la función memoria y otro para la pausa) a diferencia de la "codigo4" que tiene en uso tres pulsadores (uno para la función memoria otro para pausa y otro para la pinza)
Material:
4 potencio metros.
4 servomotores.
1 fuente de 5v.
2 pulsadores.
2 resistencias de 330.
1 arduino UNO (solo en el diagrama... en el proyecto yo utilice un arduino leonardo).
1 protoboard.
Cables.
Diagrama de conexión 2
Descripción: este es el diagrama de conexión que corresponde al código "codigo4" en el que se utilizan tres pulsadores (uno para la función memoria otro para pausa y otro para la pinza)
Material:
3 potencio metros.
4 servomotores.
1 fuente de 5v.
3 pulsadores.
3 resistencias de 330.
1 arduino UNO (solo en el diagrama... en el proyecto yo utilice un arduino leonardo).
1 protoboard.
Cables.
Diagrama de flujo
jueves, 21 de abril de 2016
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);
}
#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
#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()
{
}
Prueba 1 "movimiento"
El proyecto esta inspirado en un robot creado por el usuario "pinaut" en youtube dejo el link (en la descripción de su vídeo encontraran el código que uso).
Un día trabajando en un circuito lo vi y me dije
-si el pudo hacerlo... pues yo también-
Desarrolle mi propio código y se puede notar la diferencia en la velocidad de ejecución.
Como primer reto me decidí a crear el brazo y mover lo con programación de cada servomotor.
el segundo reto fue mover lo precisión usando el "control espejo" y el código 1.
Código 1:
#include <Servo.h>
Servo servo1;//base
Servo servo2;//codo
Servo servo3;//mano
Servo servo4;//pinza
void setup() {
servo1.attach(8);
servo2.attach(9);
servo3.attach(10);
servo4.attach(11);
Serial.begin(9600);
}
void loop() {
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);
Serial.print("angulos");
Serial.print("base");
Serial.print(paso1);
Serial.print("codo");
Serial.print(paso2);
Serial.print("mano");
Serial.print(paso3);
Serial.print("pinza");
Serial.print(paso4);
delay(25);
}
Suscribirse a:
Entradas (Atom)