151 lines
2.4 KiB
C++
151 lines
2.4 KiB
C++
|
|
||
|
|
#include "WProgram.h"
|
||
|
|
void izquierda(int iz);
|
||
|
|
void derecha(int der);
|
||
|
|
void avanza(int av);
|
||
|
|
void retrocede(int re);
|
||
|
|
void stop_b();
|
||
|
|
void stop_a();
|
||
|
|
void setup();
|
||
|
|
void loop();
|
||
|
|
int IR = 6;
|
||
|
|
int zumbador = 8;
|
||
|
|
int camara = 11;
|
||
|
|
int bit1 = 0;
|
||
|
|
int bit2 = 1;
|
||
|
|
int bit3 = 2;
|
||
|
|
int bit4 = 3;
|
||
|
|
int val1 = 0;
|
||
|
|
int val2 = 0;
|
||
|
|
int val3 = 0;
|
||
|
|
int val4 = 0;
|
||
|
|
int val = 0;
|
||
|
|
int speed1 = 9;
|
||
|
|
int speed2 = 10;
|
||
|
|
int g1a = 2;
|
||
|
|
int g1b = 3;
|
||
|
|
int g2a = 5;
|
||
|
|
int g2b = 4;
|
||
|
|
|
||
|
|
void izquierda(int iz)
|
||
|
|
{
|
||
|
|
digitalWrite(g1a, HIGH);
|
||
|
|
digitalWrite(g1b, LOW);
|
||
|
|
analogWrite(speed1, iz);
|
||
|
|
}
|
||
|
|
|
||
|
|
void derecha(int der)
|
||
|
|
{
|
||
|
|
digitalWrite(g1a, LOW);
|
||
|
|
digitalWrite(g1b, HIGH);
|
||
|
|
analogWrite(speed1, der);
|
||
|
|
}
|
||
|
|
|
||
|
|
void avanza(int av)
|
||
|
|
{
|
||
|
|
digitalWrite(g2a, HIGH);
|
||
|
|
digitalWrite(g2b, LOW);
|
||
|
|
analogWrite(speed2, av);
|
||
|
|
}
|
||
|
|
|
||
|
|
void retrocede(int re)
|
||
|
|
{
|
||
|
|
digitalWrite(g2a, LOW);
|
||
|
|
digitalWrite(g2b, HIGH);
|
||
|
|
analogWrite(speed2, re);
|
||
|
|
}
|
||
|
|
|
||
|
|
void stop_b()
|
||
|
|
{
|
||
|
|
digitalWrite(g1a, LOW);
|
||
|
|
digitalWrite(g1b, LOW);
|
||
|
|
analogWrite(speed1, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void stop_a()
|
||
|
|
{
|
||
|
|
digitalWrite(g2a, LOW);
|
||
|
|
digitalWrite(g2b, LOW);
|
||
|
|
analogWrite(speed2, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void setup()
|
||
|
|
{
|
||
|
|
Serial.begin(9600);
|
||
|
|
delay(100);
|
||
|
|
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;
|
||
|
|
val=val1+val2+val3+val4;
|
||
|
|
Serial.print("Hola, soy el robot numero ");
|
||
|
|
Serial.println(val);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop()
|
||
|
|
{
|
||
|
|
if (Serial.available()) {
|
||
|
|
val = Serial.read();
|
||
|
|
if (val == 'W') {
|
||
|
|
avanza(255);
|
||
|
|
digitalWrite(camara, HIGH);
|
||
|
|
}
|
||
|
|
if (val == 'Z') {
|
||
|
|
retrocede(255);
|
||
|
|
digitalWrite(camara, HIGH);
|
||
|
|
}
|
||
|
|
if (val == 'A') {
|
||
|
|
izquierda(255);
|
||
|
|
digitalWrite(camara, HIGH);
|
||
|
|
}
|
||
|
|
if (val == 'S') {
|
||
|
|
derecha(255);
|
||
|
|
digitalWrite(camara, HIGH);
|
||
|
|
}
|
||
|
|
if (val == 'P') {
|
||
|
|
stop_a();
|
||
|
|
digitalWrite(camara, LOW);
|
||
|
|
}
|
||
|
|
if (val == 'O') {
|
||
|
|
stop_b();
|
||
|
|
digitalWrite(camara, LOW);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
/*avanza(200);
|
||
|
|
derecha(200);
|
||
|
|
delay(1000);
|
||
|
|
retrocede(200);
|
||
|
|
izquierda(200);
|
||
|
|
delay(1000);*/
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
int main(void)
|
||
|
|
{
|
||
|
|
init();
|
||
|
|
|
||
|
|
setup();
|
||
|
|
|
||
|
|
for (;;)
|
||
|
|
loop();
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|