#include <Servo.

h>

Servo ServoBase;
Servo ServoHombro;
Servo ServoCodo;
Servo ServoMunieca;
Servo ServoGiro;
Servo ServoPinza;

int tiempo =13;

int tiempo2 =15;

int ledRojo=9;
int ledAzul=10;
int ledVerde=11;

int PinPotencBase = A0;

int PinPotencHombro = A1;
int PinPotencCodo = A2;
int PinPotencMunieca = A3;
int PinPotencPinza = A4;
int ValorPotBase;
int ValorPotHombro;
int ValorPotCodo;
int ValorPotMunieca;
int ValorPotPinza;

int BotonPrueba = 22;

int BotonLeerPociciones = 23;
int BotonLecturaCensor = 24;
int BotonMovimientoGrabado = 25;
int BotonSalir = 26;
int BotonP = 0;
int BotonL = 0;
int BotonLC = 0;
int BotonM = 0;
int BotonS = 0;

int lecturaCny=8;

int Pociciones[4][5];

bool recursivo=true;

int Opcion;

int Aumento=0;

int valorMinPinza = 10;

void setup() {
ServoBase.attach(2);
ServoHombro.attach(3);
ServoCodo.attach(4);
ServoMunieca.attach(5);
ServoPinza.attach(6);
 ServoGiro.attach(7);

pinMode(BotonPrueba,INPUT);
pinMode(BotonLeerPociciones,INPUT);
pinMode(BotonLecturaCensor,INPUT);
pinMode(BotonMovimientoGrabado,INPUT);

pinMode(ledRojo,OUTPUT);
pinMode(ledAzul,OUTPUT);
pinMode(ledVerde,OUTPUT);
digitalWrite(ledRojo,LOW);
digitalWrite(ledAzul,LOW);
digitalWrite(ledVerde,LOW);

ServoBase.write(144); //0
ServoHombro.write(116); // de 45 a 128 // 128
ServoCodo.write(166); //170
ServoMunieca.write(4); //0
ServoPinza.write(70); // 20 a 70 //7
ServoGiro.write(10); // 80 a 50 //7

Serial.begin(9600);
}

void loop() {
BotonP = digitalRead(BotonPrueba);
BotonL = digitalRead(BotonLeerPociciones);
BotonLC = digitalRead(BotonLecturaCensor);

digitalWrite(ledAzul,HIGH);

if(BotonP == HIGH)
{
digitalWrite(ledAzul,LOW);
delay(300);
Prueba();
}
if(BotonL == HIGH)
{
digitalWrite(ledAzul,LOW);
while(recursivo)
{
BotonS = digitalRead(BotonSalir);
if(BotonS==HIGH)
{
delay(300);
recursivo=false;
}
delay(100);
digitalWrite(ledVerde,HIGH);
Manejar();
}
recursivo=true;
digitalWrite(ledVerde,LOW);
}
if(BotonLC==HIGH)
{
int contador=0;
digitalWrite(ledAzul,LOW);
while(recursivo==1)
{
Serial.println(contador);
BotonS = digitalRead(BotonSalir);
BotonLC = digitalRead(BotonLecturaCensor);
if(BotonLC==HIGH)
{
delay(300);
contador= 1 - contador;
if(contador==1)
{
digitalWrite(ledRojo,LOW);
digitalWrite(ledVerde,HIGH);
delay(500);
digitalWrite(ledVerde,LOW);
delay(10);
}
if(contador==0)
{

digitalWrite(ledRojo,LOW);
digitalWrite(ledAzul,HIGH);
delay(500);
digitalWrite(ledAzul,LOW);
delay(10);
}
}
if(BotonS==HIGH)
{
recursivo=false;
}
digitalWrite(ledRojo,HIGH);
int Cny=digitalRead(lecturaCny);

if(contador==0)
{

if(Cny==HIGH)
{
delay(500);
digitalWrite(ledRojo,LOW);
MoverRecorrido();
}
}
if(contador==1)
{
if(Cny==HIGH)
{
delay(500);
digitalWrite(ledRojo,LOW);
MoverRecorrido2();
}
}
 }
recursivo=true;
digitalWrite(ledRojo,LOW);
}
}

void Prueba()
{
digitalWrite(ledVerde,HIGH);
int b(0),h(116),c(166),m(5);
for(int a=144;a>=15;a--)
{
ServoBase.write(a);
if(a<130)
{
if(h<=128)
ServoHombro.write(h++);
if(a<136)
{
if(c>=149)
ServoCodo.write(c--);
}
if(m<=6)
ServoMunieca.write(m++);
}
delay(tiempo);
}
b=15;
for(int a=30;a>=1;a--)
{
if(a>=15)
ServoBase.write(b--);
if(h>=111)
ServoHombro.write(h--);
if(c<=165)
ServoCodo.write(c++);
if(m<=12)
ServoMunieca.write(m++);
delay(tiempo);
}
delay(100);
for(int a=60;a>=valorMinPinza;a--)
{
ServoPinza.write(a);
delay(tiempo);
}
digitalWrite(ledVerde,LOW);

//////////////////////////////////////////

for(int repeti=0;repeti<2;repeti++)
{
digitalWrite(ledRojo,HIGH);
for(int a=0,b=180;a<180;a++,b--)
{
ServoBase.write(a);
if(a>=45 && a<=111)
ServoHombro.write(b);
if(b<165)
 ServoCodo.write(b);
if(a>=12)
ServoMunieca.write(a);

if(a>=0)
ServoGiro.write(a);
delay(tiempo2);
}
for(int a=valorMinPinza;a<=60;a++)
{
ServoPinza.write(a);
delay(tiempo);
}
for(int a=180,b=1;a>=1;a--,b++)
{
ServoBase.write(a);
if(a>=45 && a<=111)
ServoHombro.write(b);
if(b<=165)
ServoCodo.write(b);
if(a>=12)
ServoMunieca.write(a);
if(a>=0)
ServoGiro.write(a);
delay(tiempo2);
}
for(int a=60;a>=valorMinPinza;a--)
{
ServoPinza.write(a);
delay(tiempo);
}
digitalWrite(ledRojo,LOW);
}

/////////////////////////////////////////////
digitalWrite(ledVerde,HIGH);
b=1;
c=151;
h=125;
m=12;
for(int a=1;a<=130;a++)
{
ServoBase.write(a);
if(h<=128)
ServoHombro.write(h++);
if(c>=149)
ServoCodo.write(c--);
if(m>=6)
ServoMunieca.write(m--);
delay(tiempo);
}
for(int a=130;a<=144;a++)
{
ServoBase.write(a);
if(h>=116)
ServoHombro.write(h--);
if(c<=166)
ServoCodo.write(c++);
if(m<=12)
 ServoMunieca.write(m--);
delay(tiempo);
}
delay(100);
for(int a=valorMinPinza;a<=60;a++)
{
ServoPinza.write(a);
delay(tiempo);
}
digitalWrite(ledVerde,LOW);
}

void Manejar()
{

ValorPotBase = analogRead(PinPotencBase);
ValorPotHombro = analogRead(PinPotencHombro);
ValorPotCodo = analogRead(PinPotencCodo);
ValorPotMunieca = analogRead(PinPotencMunieca);
ValorPotPinza = analogRead(PinPotencPinza);

ValorPotBase = map(ValorPotBase,0,1023,0,180);
ValorPotHombro = map(ValorPotHombro,0,1023,0,180);
ValorPotCodo = map(ValorPotCodo,0,1023,0,180);
ValorPotMunieca = map(ValorPotMunieca,0,1023,0,180);
ValorPotPinza = map(ValorPotPinza,0,1023,0,180);

ServoBase.write(ValorPotBase);
ServoHombro.write(ValorPotHombro);
ServoCodo.write(ValorPotCodo);
ServoMunieca.write(ValorPotMunieca);
ServoPinza.write(ValorPotPinza);
Serial.print(ValorPotBase);
Serial.print(" ");
Serial.print(ValorPotHombro);
Serial.print(" ");
Serial.print(ValorPotCodo);
Serial.print(" ");
Serial.print(ValorPotMunieca);
Serial.println(" ");

delay(tiempo);
}

void MoverRecorrido()
{
digitalWrite(ledVerde,HIGH);
int b(0),h(116),c(166),m(5),aumento(0);
for(int a=144;a>=15;a--)
{
ServoBase.write(a);
if(a<130)
{
if(h<=128)
ServoHombro.write(h++);
if(a<136)
{
if(c>=149)
 ServoCodo.write(c--);
}
if(m<=6)
ServoMunieca.write(m++);
}
delay(tiempo);
}
b=15;
for(int a=30;a>=1;a--)
{
if(a>=15)
ServoBase.write(b--);
if(h>=111)
ServoHombro.write(h--);
if(c<=165)
ServoCodo.write(c++);
if(m<=12)
ServoMunieca.write(m++);
delay(tiempo);
}

delay(100);
for(int a=60;a>=valorMinPinza;a--)
{
ServoPinza.write(a);

delay(tiempo);
}
////////////////////////////////////
b=1;
c=165;
h=111;
m=12;
for(int a=1;a<=130;a++)
{
ServoBase.write(a);
if(h<=128)
ServoHombro.write(h++);
if(c>=149)
ServoCodo.write(c--);
if(m>=6)
ServoMunieca.write(m--);
delay(tiempo);
}
for(int a=130;a<=144;a++)
{
ServoBase.write(a);
if(h>=116 || a<=144)
ServoHombro.write(h--);
if(c<=166)
ServoCodo.write(c++);
if(m<=6)
ServoMunieca.write(m--);
delay(tiempo);
}
delay(100);
for(int a=valorMinPinza;a<=60;a++)
{
ServoPinza.write(a);
 delay(tiempo);
}
digitalWrite(ledVerde,LOW);
}

void MoverRecorrido2()
{
digitalWrite(ledVerde,HIGH);
int b(0),h(116),c(166),m(5),aumento(0);

for(int a=144;a>=15;a--)
{
ServoBase.write(a);
if(a<130)
{
if(h<=128)
ServoHombro.write(h++);
if(a<136)
{
if(c>=149)
ServoCodo.write(c--);
}
if(m<=6)
ServoMunieca.write(m++);
}
delay(tiempo);
}
b=15;
for(int a=65;a>=1;a--)
{
if(a>=15)
ServoBase.write(b--);
if(h>=111)
ServoHombro.write(h--);
if(c<=165)
ServoCodo.write(c++);
if(m<=12)
ServoMunieca.write(m++);
delay(tiempo);
}

delay(100);
for(int a=60;a>=valorMinPinza;a--)
{
ServoPinza.write(a);

delay(tiempo);
}
////////////////////////////////////
b=1;
c=165;
h=111;
m=12;
for(int a=1;a<=70;a++)
{
ServoBase.write(a);
if(h<=120)
ServoHombro.write(h++);
 if(c>=120)
ServoCodo.write(c--);
if(m<=35)
ServoMunieca.write(m++);

ServoGiro.write(a);
delay(tiempo);
}
for(int a=70;a<=180;a++)
{
ServoGiro.write(a);
delay(tiempo/2);
}
delay(300);
for(int a=valorMinPinza;a<=60;a++)
{
ServoPinza.write(a);
delay(tiempo);
}
for(int a=180;a>=140;a--)
{
ServoGiro.write(a);
delay(tiempo);
}
int g=70;
c=120;
h=120;
m=35;
for(int a=70;a<=144;a++)
{

ServoBase.write(a);
if(h>=116)
ServoHombro.write(h--);
if(c<=166)
ServoCodo.write(c++);
if(m>=6)
ServoMunieca.write(m--);
if(g>=0)
ServoGiro.write(g--);
delay(tiempo);
}
delay(100);

digitalWrite(ledVerde,LOW);
}