142 lines
2 KiB
C++
142 lines
2 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 = 6;
|
||
|
|
int zumbador =11;
|
||
|
|
int camara = 8;
|
||
|
|
int girocam = 3;
|
||
|
|
int bit1 = 0;
|
||
|
|
int bit2 = 1;
|
||
|
|
int bit3 = 2;
|
||
|
|
int bit4 = 3;
|
||
|
|
int val1 = 0;
|
||
|
|
int val2 = 0;
|
||
|
|
int val3 = 0;
|
||
|
|
int val4 = 0;
|
||
|
|
byte val = 0;
|
||
|
|
int speed1 = 9;
|
||
|
|
int speed2 = 10;
|
||
|
|
int id = 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(9600);
|
||
|
|
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=16;
|
||
|
|
else val1=0;
|
||
|
|
if (analogRead(bit2)>512) val2=32;
|
||
|
|
else val2=0;
|
||
|
|
if (analogRead(bit3)>512) val3=64;
|
||
|
|
else val3=0;
|
||
|
|
if (analogRead(bit4)>512) val4=128;
|
||
|
|
else val4=0;
|
||
|
|
id=val1+val2+val3+val4;
|
||
|
|
Serial.print("Hola, soy el robot numero ");
|
||
|
|
Serial.println(id);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop()
|
||
|
|
{
|
||
|
|
|
||
|
|
for(val = 1 ; val <= 180; val++)
|
||
|
|
{
|
||
|
|
servoC.write(val);
|
||
|
|
Servo::refresh();
|
||
|
|
delay(15);
|
||
|
|
}
|
||
|
|
for(val = 180 ; val > 0; val--)
|
||
|
|
{
|
||
|
|
servoC.write(val);
|
||
|
|
Servo::refresh();
|
||
|
|
delay(15);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
int main(void)
|
||
|
|
{
|
||
|
|
init();
|
||
|
|
|
||
|
|
setup();
|
||
|
|
|
||
|
|
for (;;)
|
||
|
|
loop();
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|