#include <math.h>

//Entradas digitales
#define R3 2
#define L3 3

//Entradas analogicas

#define iz_ejex 2
#define iz_ejey 3
#define der_ejey 4
#define der_ejex 5
#define R2 7
#define L2 1
#define R1 6
#define L1 0


#define ret 3

int servo_hor=0;
int servo_hor_ant=0;

int servo_ver=0;
int servo_ver_ant=0;

int med_iz_ejex = 0x00;
int med_iz_ejey = 0x00;

int med_der_ejex = 0x00;
int med_der_ejey = 0x00;

int val_derx = 0x00; //Valor entrada serie
int val_derx_ant = 0x01; //Valor entrada serie
float val_izy = 0; //Valor entrada serie
int val_izy_ant = 0x01; //Valor entrada serie
float val_izx = 0; //Valor entrada serie
int val_izx_ant = 0x01; //Valor entrada serie
int oruga_der = 0x00;
int oruga_iz = 0x00;

double vel = 1;
double ang = 1;
int angulo = 0;
int angulo_ant=1;

int flagL2=0;
int flagR2=0;

int flagL1=0;
int flagR1=0;

int medR2=0;
int medL2=0;

int medR1=0;
int medL1=0;

int angulo_ant_mas=0;
int angulo_ant_men=0;

void setup()                    
{
  Serial.begin(19200); 
    pinMode(14, INPUT);
    pinMode(15, INPUT);
    digitalWrite(14,HIGH);
    digitalWrite(15,HIGH);
    pinMode(20, INPUT);
    pinMode(21, INPUT);
    digitalWrite(20,HIGH);
    digitalWrite(21,HIGH);
    med_iz_ejex = analogRead(iz_ejex);
    med_iz_ejey = analogRead(iz_ejey);
    
    med_der_ejex = analogRead(der_ejex);
    med_der_ejey = analogRead(der_ejey);
    
    medR1=analogRead(R1)/2;
    medL1=analogRead(L1)/2;
    
    medR2=analogRead(R2)/2;
    medL2=analogRead(L2)/2;
    
}

void loop()                      //run over and over again
{

  val_izy=analogRead(iz_ejey);  
  val_izx=analogRead(iz_ejex);
  
  if ((val_izx<(med_iz_ejex-100))&&(abs(val_izx_ant-val_izx)>=2))
  {
    val_izx_ant=val_izx;
    transmite(0x07,map(val_izx,med_iz_ejex-100,0,0,127));
  }
  else if ((val_izx>(med_iz_ejex+100))&&(abs(val_izx_ant-val_izx)>=2))
  {
    val_izx_ant=val_izx;
    transmite(0x07,map(val_izx,med_iz_ejex+100,1023,129,255));
  }
  else if (abs(val_izx_ant-val_izx)>=2) 
  {
    val_izx_ant=val_izx;
    transmite(0x07,128);
  }
  
  servo_hor=map(analogRead(der_ejex), 0, 1023, 0, 180);
  
  if (abs(servo_hor-servo_hor_ant)>2)
  {
    servo_hor_ant=servo_hor;
    transmite(0x0B,servo_hor);
  }
  
  servo_ver=map(analogRead(der_ejey), 0, 1023, 0, 180);
  if (abs(servo_ver-servo_ver_ant)>2)
  {
    servo_ver_ant=servo_ver;
    transmite(0x0A,servo_ver);
  }
  
  if ((analogRead(L2)<medL2)&&(flagL2==0))
  {
    flagL2=1;
    transmite(0x01,0xFF);
  }
  else if((analogRead(L2)>medL2)&&(flagL2==1))
  {
    flagL2=0;
    transmite(0x01,0x00);
  }
  
   if ((analogRead(R2)<medR2)&&(flagR2==0))
  {
    flagR2=1;
    transmite(0x00,0xFF);
  }
  else if((analogRead(R2)>medR2)&&(flagR2==1))
  {
    flagR2=0;
    transmite(0x00,0x00);
  }
 
  if ((analogRead(R1)<medR1)&&(flagR1==0)) //CAMARA ON
  {
    flagR1=1;
    transmite(0x03,0x01);
    transmite(0x03,0x01);
    transmite(0x03,0x01);
  }
  else if (analogRead(R1)>medR1) flagR1=0;
  
  if((analogRead(L1)<medL1)&&(flagL1==0)) //CAMARA OFF
  {
    flagL1=1;
    transmite(0x03,0x00);
    transmite(0x03,0x00);
    transmite(0x03,0x00);
  }
  else if (analogRead(L1)>medL1) flagL1=0; 
}

void transmite(int command, int data)
{
  Serial.print(0xF0, BYTE);
  Serial.print(command, BYTE);
  Serial.print((data>>4)&0x0F, BYTE);
  Serial.print(0x0F&data, BYTE);
}