240 lines
4.4 KiB
C++
240 lines
4.4 KiB
C++
|
|
#include <Servo.h>
|
|
#undef int
|
|
#undef abs
|
|
#undef double
|
|
#undef float
|
|
#undef round
|
|
|
|
#include "WProgram.h"
|
|
void stop();
|
|
void izquierda(int iz);
|
|
void derecha(int der);
|
|
void retrocede(int re);
|
|
void avanza(int av);
|
|
void setup();
|
|
void loop();
|
|
Servo servoA, servoB, servoC;
|
|
|
|
int IR = 8;
|
|
int zumbador =11;
|
|
int camara = 4;
|
|
int girocam = 2;
|
|
int bit1 = 0;
|
|
int bit2 = 1;
|
|
int bit3 = 2;
|
|
int bit4 = 3;
|
|
int led =3;
|
|
int laser =5;
|
|
int act_serv = 7;
|
|
int speed1 = 9;
|
|
int speed2 = 10;
|
|
|
|
int id = 0;
|
|
int val1 = 0;
|
|
int val2 = 0;
|
|
int val3 = 0;
|
|
int val4 = 0;
|
|
byte val = 0;
|
|
unsigned long time=0;
|
|
unsigned long time_ant=0;
|
|
int frec = 0;
|
|
int activa=0;
|
|
|
|
void stop()
|
|
{
|
|
servoA.write(91); //Parado
|
|
servoB.write(90); //Parado
|
|
}
|
|
|
|
void izquierda(int iz)
|
|
{
|
|
servoA.write(iz);
|
|
}
|
|
|
|
void derecha(int der)
|
|
{
|
|
servoB.write(der);
|
|
}
|
|
|
|
void retrocede(int re)
|
|
{
|
|
stop();
|
|
servoA.write(re);
|
|
servoB.write(180-re);
|
|
}
|
|
|
|
void avanza(int av)
|
|
{
|
|
stop();
|
|
servoA.write(180-av);
|
|
servoB.write(av);
|
|
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(19200);
|
|
delay(100);
|
|
|
|
analogWrite(zumbador,0);
|
|
|
|
servoA.attach(speed1);
|
|
//servo1.setMaximumPulse(2200);
|
|
servoB.attach(speed2);
|
|
servoC.attach(girocam);
|
|
|
|
stop();
|
|
|
|
servoC.write(93); //Parado
|
|
|
|
pinMode(IR, OUTPUT);
|
|
pinMode(zumbador, OUTPUT);
|
|
pinMode(camara, OUTPUT);
|
|
pinMode(14, INPUT);
|
|
pinMode(15, INPUT);
|
|
pinMode(16, INPUT);
|
|
pinMode(17, INPUT);
|
|
digitalWrite(14, HIGH);
|
|
digitalWrite(15, HIGH);
|
|
digitalWrite(16, HIGH);
|
|
digitalWrite(17, HIGH);
|
|
if (analogRead(bit1)>512) val1=1;
|
|
else val1=0;
|
|
if (analogRead(bit2)>512) val2=2;
|
|
else val2=0;
|
|
if (analogRead(bit3)>512) val3=4;
|
|
else val3=0;
|
|
if (analogRead(bit4)>512) val4=8;
|
|
else val4=0;
|
|
id=val1+val2+val3+val4;
|
|
Serial.print("Hola, soy el robot numero ");
|
|
Serial.println(id);
|
|
digitalWrite(camara, LOW); //Desactiva camara
|
|
digitalWrite(act_serv, HIGH); //Desactiva servomotores
|
|
digitalWrite(led, LOW); //Apaga leds
|
|
digitalWrite(laser, LOW); //Apaga laser
|
|
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
Servo::refresh();
|
|
if (Serial.available()) {
|
|
//Serial.print(val, BYTE);
|
|
val = Serial.read();
|
|
if ((val>>4)==id)
|
|
{
|
|
Serial.print(val, BYTE);
|
|
|
|
switch (val&0x0F) {
|
|
case 0x00: //Servo derecho avanza
|
|
digitalWrite(act_serv, LOW);
|
|
derecha(180);
|
|
break;
|
|
case 0x01: //Servo izquierdo avanza
|
|
digitalWrite(act_serv, LOW);
|
|
izquierda(0);
|
|
break;
|
|
case 0x02: //Servo derecho retrocede
|
|
{
|
|
digitalWrite(act_serv, LOW);
|
|
derecha(0);
|
|
}
|
|
break;
|
|
case 0x03: //Servo izquierdo retrocede
|
|
{
|
|
digitalWrite(act_serv, LOW);
|
|
izquierda(180);
|
|
}
|
|
break;
|
|
case 0x04:
|
|
{
|
|
frec= 100;
|
|
activa=1;
|
|
time_ant=millis();
|
|
}
|
|
break;
|
|
case 0x05:
|
|
{
|
|
activa=0;
|
|
analogWrite(zumbador,0);
|
|
}
|
|
break;
|
|
case 0x06: //IR ON
|
|
{
|
|
digitalWrite(IR, HIGH);
|
|
}
|
|
break;
|
|
case 0x07: //IR OFF
|
|
{
|
|
digitalWrite(IR, LOW);
|
|
}
|
|
break;
|
|
case 0x08: //laser ON
|
|
{
|
|
digitalWrite(laser, HIGH);
|
|
digitalWrite(led, HIGH); //Apaga leds
|
|
}
|
|
break;
|
|
case 0x09: //laser OFF
|
|
{
|
|
digitalWrite(laser, LOW);
|
|
digitalWrite(led, LOW); //Apaga leds
|
|
}
|
|
break;
|
|
case 0x0A: //Camara derecha
|
|
{
|
|
digitalWrite(act_serv, LOW);
|
|
servoC.write(180);
|
|
}
|
|
break;
|
|
case 0x0B: //Camara izquierda
|
|
{
|
|
digitalWrite(act_serv, LOW);
|
|
servoC.write(0);
|
|
}
|
|
break;
|
|
case 0x0C: //Parada de los servomotores
|
|
digitalWrite(act_serv, HIGH);
|
|
stop();
|
|
break;
|
|
case 0x0D: //Enciende camara
|
|
digitalWrite(camara, HIGH);
|
|
break;
|
|
case 0x0E: //Apaga camara
|
|
digitalWrite(camara, LOW);
|
|
break;
|
|
case 0x0F: //Camara centrada
|
|
digitalWrite(act_serv, LOW);
|
|
servoC.write(93); //Parado
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
time = millis();
|
|
if (activa==1)
|
|
{
|
|
if (time-time_ant >= frec)
|
|
{
|
|
analogWrite(zumbador,255);
|
|
if (time-time_ant >= 2*frec) time_ant=time;
|
|
}
|
|
else analogWrite(zumbador,0);
|
|
}
|
|
}
|
|
|
|
|
|
int main(void)
|
|
{
|
|
init();
|
|
|
|
setup();
|
|
|
|
for (;;)
|
|
loop();
|
|
|
|
return 0;
|
|
}
|
|
|