El código 7 utiliza el teclado de la PC para mover y memorizar posiciones así como pausar y guardar el robot, se usa de forma similar al código 8.
Solo utiliza los servos y una fuente de alimentación de 5v 4 2000mA.
/*
comando
'1' base 'd' +5 'm' memorias 'p' pause 'e' pinza
'2' motor 2 'f' +10 'a' autómata 'p' reanudar
'3' motor 3 'c' -5 'g' guardar
'4' muñeca 'v' -10
'5' pinza
*/
#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 20 posiciones con esta configuración
byte memoria1[20];
byte memoria2[20];
byte memoria3[20];
byte memoria4[20];
byte memoria5[20];
//arreglo para movimiento fuera de autómata
byte paso[5];
//variables para entrada serial
byte s;
unsigned char comando=20;
//arreglos para cálculos
double registro[5], direccion[5], diferencia[5];
//contadores
byte figura,pasomax,seccion=1,limite;
//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, primer_seccion=true, ultima_memoria=true, pause=false, pinza=true ;
void setup()
{
pinMode(13, OUTPUT);
servo1.attach(8);
servo2.attach(9);
servo3.attach(10);
servo4.attach(11);
servo5.attach(12);
paso[0] = 90;
paso[1] = 80;
paso[2] =120;
paso[3] = 80;
paso[4] = 90;
Serial.begin(115200);
Serial.print("\t\tIniciando | ");
for(s=0;s<31;s++){
Serial.print("###");
delay(25);
}
s=0;
Serial.print(" | 100% 0.75s");
Serial.print("\n\n\t\tBIENVENIDO USUARIO :)\n");
delay(500);
Serial.print("\t\t''CÓDIGO 7'' CONTROL SERIAL-JUALLEK 2016");
}
void loop()
{
tiempotranscurrido=millis();
tiempomicros=micros();
serial();
if(!automata)
{
mover();
}
else if(automata)
{
if(primer_paso==true)
{
if(ultima_memoria)ultimo_registro();
figura=0;
primer_paso=false;
}
else if(figura>pasomax)
{
figura=0;
}
if(primer_seccion)matriz_de_traslacion();
if(tiempomicros-tiempob>time)
{
tiempob=tiempomicros;
ejecutar_automata();
}
}
}
void estado()
{
if(comando=='m')
{
figura++;
if(primer_memoria==true)figura=0;
memorisa();
}
else if(comando=='a')
{
automata=true;
Serial.print("\n\t\t****************************************EJECUTAR_AUTOMATA****************************************");
}
}
void mover()
{
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\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]);
}
else
{
limite=0;
seccion=1;
figura++;// figura al pasar a la siguiente memoria >>
primer_seccion=true;
}
while(pause==true)
{
if(Serial.available())
{
comando=Serial.read();
if (comando=='p')pause=false;
}
if(comando=='g')// En pausa el robot se puede colocar recto para guardarlo y poder mover lo de lugar
{
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 CALCULO DE MATRIZ ");Serial.print(figura);
//calcular diferencias
diferencia[0] = abs (registro[0]-memoria1[figura]);
diferencia[1] = abs (registro[1]-memoria2[figura]);
diferencia[2] = abs (registro[2]-memoria3[figura]);
diferencia[3] = abs (registro[3]-memoria4[figura]);
diferencia[4] = abs (registro[4]-memoria5[figura]);
// se elimino la sección de filtrado (no hay vibraciones causadas por los potencio metros)
//calcular limite de secciones (la mayor distancia)
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 LIMITE=");Serial.print(limite);
//calcula dirección e incremento en grados de cada sección
if(registro[0]>memoria1[figura]) direccion[0]=(0-diferencia[0])/limite; else
direccion[0]=diferencia[0]/limite;
if(registro[1]>memoria2[figura]) direccion[1]=(0-diferencia[1])/limite; else
direccion[1]=diferencia[1]/limite;
if(registro[2]>memoria3[figura]) direccion[2]=(0-diferencia[2])/limite; else
direccion[2]=diferencia[2]/limite;
if(registro[3]>memoria4[figura]) direccion[3]=(0-diferencia[3])/limite; else
direccion[3]=diferencia[3]/limite;
if(registro[4]>memoria5[figura]) direccion[4]=(0-diferencia[4])/limite; else direccion[4]=diferencia[4]/limite;
Serial.print("\n\t\t\t\t\t\t\t\tEJECUTANDO_MEMORIA ");Serial.print(figura);
primer_seccion=false;
}
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 memorisa()
{
Serial.print("\n\t\t\t\t\t\tMEMORIA\t\t"); Serial.print(figura);
memoria1[figura] = paso[0];
memoria2[figura] = paso[1];
memoria3[figura] = paso[2];
memoria4[figura] = paso[3];
memoria5[figura] = paso[4];
pasomax=figura;
primer_memoria=false;
digitalWrite(13,HIGH);
delay(100);
digitalWrite(13,LOW);
}
void entrada_serial()
{
comando=Serial.read();
if(comando=='1')s=0;
if(comando=='2')s=1;
if(comando=='3')s=2;
if(comando=='4')s=3;
if(comando=='5')s=4;
}
void asignacion()
{
if(s==4)
{
if(comando=='e'&&pinza==true)
{
paso[4]=130;
pinza=false;
}
else if(comando=='e')
{
paso[4]=90;
pinza=true;
}
}
if(paso[s]<180){
if(comando=='d')paso[s]+=5;
if(comando=='f')paso[s]+=10;
}
if(paso[s]>0){
if(comando=='c')paso[s]-=5;
if(comando=='v')paso[s]-=10;
}
if(paso[s]>=180)paso[s]=180;
else if(paso[s]<=0)paso[s]=0;
}
void serial()
{
if(Serial.available())
{
entrada_serial();
asignacion();
estado();
if(comando=='p')pause=true;
}
}
No hay comentarios.:
Publicar un comentario