#import<Servo.h>
//VARIABLES SENSORES ULTRASONICOS ---------
const int trigSen1= 2;//TriggerPin Sensores
const int echoSen1= 3;//EchoPin Sensores
float duracion; //Duracion entre trigger y echo
float distancia; //Distancia del objeto Sensor1
float distancia2; //Duracion del sonido Sensor2
const int rango= 50;//Rango para detectar objetos en cm
//VARIABLES JOYSTICK
int joyStickX= 0;//Pin analogico 0 para eje X
int joyStickY= 1;//Pin analogico 1 para eje Y
int joyStickV; //Leer los valores
int joyStickBoton= 9;//Boton del joystick
//SERVOMOTORES----------------------
const int servo2Pin = 7;//Pin del servo eje Y
const int servoPin = 6;//Pin del Servo eje X
const int gatilloPin = 8;//Pin del servo del gatillo
int servoPos= 90;//Posicion del Servo eje X
int servo2Pos= 90;//Posicion del servo eje Y
int servoInc= 4;//Incremento del Servo eje X
//Inicializacion de los servos
Servo servo;
Servo servo2;
Servo gatillo;
//VARIABLES GENERALES---------------------
bool serial= true; //Activar Serial
int direccion= 0;//0->No objeto. 1->Objeto a la derecha. 2.-Objeto a la izquierda
int core = 0;//State Machine, 0->Centinela 1->Ataque 2->Manual
int buzzPin= 9;//Pin del Buzzer
//MUSICA --------------------------------
#define NOTE_F4 349//
#define NOTE_GS4 415//
#define NOTE_A4 440//
#define NOTE_B4 494//
#define NOTE_C5 523//
#define NOTE_CS5 554//
#define NOTE_D5 587//
#define NOTE_DS5 622//
#define NOTE_E5 659//
#define NOTE_F5 698//
#define NOTE_G5 784//
#define NOTE_GS5 831//
#define NOTE_A5 880//
#define REST 0
int tempo = 120;
int melody[] = {
NOTE_A4,-4, NOTE_A4,-4, NOTE_A4,16, NOTE_A4,16, NOTE_A4,16, NOTE_A4,16, NOTE_F4,8, REST,8,
NOTE_A4,-4, NOTE_A4,-4, NOTE_A4,16, NOTE_A4,16, NOTE_A4,16, NOTE_A4,16, NOTE_F4,8, REST,8,
NOTE_A4,4, NOTE_A4,4, NOTE_A4,4, NOTE_F4,-8, NOTE_C5,16,
NOTE_A4,4, NOTE_F4,-8, NOTE_C5,16, NOTE_A4,2,//4
NOTE_E5,4, NOTE_E5,4, NOTE_E5,4, NOTE_F5,-8, NOTE_C5,16,
NOTE_A4,4, NOTE_F4,-8, NOTE_C5,16, NOTE_A4,2,
NOTE_A5,4, NOTE_A4,-8, NOTE_A4,16, NOTE_A5,4, NOTE_GS5,-8, NOTE_G5,16, //7
NOTE_DS5,16, NOTE_D5,16, NOTE_DS5,8, REST,8, NOTE_A4,8, NOTE_DS5,4, NOTE_D5,-8, NOTE_CS5,16,
NOTE_C5,16, NOTE_B4,16, NOTE_C5,16, REST,8, NOTE_F4,8, NOTE_GS4,4, NOTE_F4,-8, NOTE_A4,-16,//9
NOTE_C5,4, NOTE_A4,-8, NOTE_C5,16, NOTE_E5,2,
NOTE_A5,4, NOTE_A4,-8, NOTE_A4,16, NOTE_A5,4, NOTE_GS5,-8, NOTE_G5,16, //7
NOTE_DS5,16, NOTE_D5,16, NOTE_DS5,8, REST,8, NOTE_A4,8, NOTE_DS5,4, NOTE_D5,-8, NOTE_CS5,16,
NOTE_C5,16, NOTE_B4,16, NOTE_C5,16, REST,8, NOTE_F4,8, NOTE_GS4,4, NOTE_F4,-8, NOTE_A4,-16,//9
NOTE_A4,4, NOTE_F4,-8, NOTE_C5,16, NOTE_A4,2,
};
int notes = sizeof(melody) / sizeof(melody[0]) / 2;
int wholenote = (60000 * 4) / tempo;
int divider = 0, noteDuration = 0;
//#######################SET UP Y LOOP##########################################
//SET UP---------------------
void setup() {
//InicializarPinModes
pinMode(trigSen1, OUTPUT);
pinMode(echoSen1, INPUT);
pinMode(trigSen1+2, OUTPUT);
pinMode(echoSen1+2, INPUT);
pinMode(buzzPin,OUTPUT);
//Inizializar Servos, Motor y Serial
servo.attach(servoPin);
servo2.attach(servo2Pin);
gatillo.attach(gatilloPin);
servo.write(servoPos);
servo2.write(servo2Pos);
gatillo.write(0);
Serial.begin(9600);
delay(100);
for (int thisNote = 0; thisNote < notes * 2; thisNote = thisNote + 2) {
divider = melody[thisNote + 1];
if (divider > 0) {
noteDuration = (wholenote) / divider;
} else if (divider < 0) {
noteDuration = (wholenote) / abs(divider);
noteDuration *= 1.5;
}
tone(buzzPin, melody[thisNote], noteDuration*0.9);
delay(noteDuration);
noTone(buzzPin);
}
}
//LOOP-----------------------
void loop() {
switch(core){
case 0:centinela();break;
case 1:modoAtaque();break;
case 2:modoManual();break;
}
if (Serial.available() > 0) {
char entrada = Serial.read();
if (entrada == 'M') {
if(core!=2){
core=2;
}else{
core=0;
}
}
if(entrada == 'F'){
gatillo.write(180);
delay(500);
gatillo.write(0);
}
if(entrada == 'R'){
for(int i=0;i<10;i++){
gatillo.write(180);
delay(500);
gatillo.write(0);
delay(500);
}
}
}
}
//###############################################################################
//MODO CENTINELA---------------------------------------------
void centinela(){
distancia=obtDistancia(0);
distancia2=obtDistancia(2);
if(distancia <=rango || distancia2 <= rango){//distancia <=rango || distancia2 <= rango
if(servoInc<0){
servoInc*=1;
}
core=1;
}else{
servoPos+=servoInc;
if(servoPos>175 || servoPos<5){
servoPos-=servoInc;
servoInc=-servoInc;
}
servo.write(servoPos);
}
}
//OBTENER DISTANCIA--------------------------------------
float obtDistancia(int sens){
digitalWrite(trigSen1+sens, LOW);
delayMicroseconds(2);
digitalWrite(trigSen1+sens, HIGH);
delayMicroseconds(10);
digitalWrite(trigSen1+sens, LOW);
duracion = pulseIn(echoSen1+sens, HIGH);
delay(50);
duracion = duracion*0.034/2;
Serial.println(duracion);
return duracion;
}
//MODO ATAQUE---------------------------------modo en el que se centra en el objetivo y se mueve con el
void modoAtaque(){
distancia=obtDistancia(0); //Primero a centrar el objeto.
distancia2=obtDistancia(2);
if(distancia-distancia2>5 && distancia2<=rango){
Serial.println("OBJETO A LA DERECHA");
servoPos-=servoInc;
if(servoPos<10 ){
servoPos+=servoInc;
}
direccion=1;
}
else if(distancia2-distancia>5 && distancia<=rango){
Serial.println("OBJETO A LA IZQUIERDA");
servoPos+=servoInc;
if(servoPos>175 ){servoPos-=servoInc;}
direccion=2;
}
else if(distancia<=rango && distancia2<=rango){
Serial.println("OBJETO EN EL CENTRO");
gatillo.write(180);
delay(500);
gatillo.write(0);
}
else{
Serial.print("OBJETO PERDIDO ");
Serial.println(direccion);
if(direccion==1){
servoPos-=servoInc;
}else if(direccion==2){
servoPos+=servoInc;
}
if(servoPos>=170 || servoPos<=10 || direccion==0){
core=0;
}
}
servo.write(servoPos);
}
void modoManual(){
joyStickV=analogRead(joyStickX);
servoPos=map(joyStickV,0,1023,0,180);
servo.write(servoPos);
joyStickV=analogRead(joyStickY);
servo2Pos=map(joyStickV,0,1023,0,180);
servo2.write(servo2Pos);
delay(20);
}