173 lines
3.3 KiB
Arduino
173 lines
3.3 KiB
Arduino
|
#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);
|
||
|
|
}
|
||
|
|
|