#include <SoftwareSerial.h>
SoftwareSerial BT(13, 12);
#include <Adafruit_LEDBackpack.h>
Adafruit_8x16matrix matrix = Adafruit_8x16matrix();
static const uint8_t PROGMEM
smile_bmp[] = { B00000000, B00000000, B00000000, B10000001, B10000001, B01000010, B00100100, B00011000 },
frown_bmp[] = { B0000000, B00000000, B00000000, B00011000, B00100100, B01000010, B10000001, B10000001 };
#include <Servo.h>
Servo servoDI, servoDD, servoTI, servoTD;
#define trigPin 7
#define echoPin 6
int limite = 200;
x = 50;
char r;
void setup() {
servoDI.attach(2);
servoDI.write(90);
servoDD.attach(3);
servoDD.write(90);
servoTI.attach(4);
servoTI.write(90);
servoTD.attach(5);
servoTD.write(90);
Serial.begin(9600);
BT.begin(9600);
matrix.begin(0x70);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}
void loop() {
if (BT.available()) {
r = BT.read();
}
if (r == ‘1’) {
avance(x);
} else {
if (r == ‘2’) {
atras(x);
} else {
if (r == ‘3’) {
izquierda(x);
} else {
if (r == ‘4’) {
derecha(x);
} else {
if (r == ‘5’) {
parar();
} else {
if (r == ‘6’) {
esquina_izquierda_sup(x);
} else {
if (r == ‘7’) {
esquina_derecha_sup(x);
} else {
if (r == ‘8’) {
esquina_izquierda_inf(x);
} else {
if (r == ‘9’) {
esquina_derecha_inf(x);
} else {
if (r == ‘B’) {
giro_eje_izquierda(x);
} else {
if (r == ‘A’) {
giro_eje_derecha(x);
}
}
}
}
}
}
}
}
}
}
}
}
void cara_feliz() {
matrix.setRotation(1);
matrix.clear();
delay(10);
matrix.drawBitmap(4, 0, smile_bmp, 8, 8, LED_ON);
matrix.writeDisplay();
delay(10);
}
void cara_triste() {
matrix.setRotation(1);
matrix.clear();
delay(10);
matrix.drawBitmap(4, 0, frown_bmp, 8, 8, LED_ON);
matrix.writeDisplay();
delay(10);
}
void no_cara() {
matrix.clear();
delay(500);
}
void parar() {
servoDI.write(90);
servoDD.write(90);
servoTI.write(90);
servoTD.write(90);
}
void avance(int v) {
cara_feliz();
servoDI.write(90 + v + 7);
servoDD.write(90 – v);
servoTI.write(90 + v + 7);
servoTD.write(90 – v);
}
void atras(int v) {
cara_triste();
servoDI.write(90 – v);
servoDD.write(90 + v);
servoTI.write(90 – v);
servoTD.write(90 + v);
}
void izquierda(int v) {
servoDI.write(90 – v);
servoDD.write(90 – v);
servoTI.write(90 + v);
servoTD.write(90 + v);
}
void derecha(int v) {
servoDI.write(90 + v);
servoDD.write(90 + v);
servoTI.write(90 – v);
servoTD.write(90 – v);
}
void esquina_derecha_inf(int v) {
servoDI.write(90);
servoDD.write(90 + v);
servoTI.write(90 – v);
servoTD.write(90);
}
void esquina_izquierda_sup(int v) {
servoDI.write(90);
servoDD.write(90 – v);
servoTI.write(90 + v);
servoTD.write(90);
}
void esquina_izquierda_inf(int v) {
servoDI.write(90 – v);
servoDD.write(90);
servoTI.write(90);
servoTD.write(90 + v);
}
void esquina_derecha_sup(int v) {
servoDI.write(90 + v);
servoDD.write(90);
servoTI.write(90);
servoTD.write(90 – v);
}
void giro_eje_izquierda(int v) {
servoDI.write(90 – v);
servoDD.write(90 – v);
servoTI.write(90 – v);
servoTD.write(90 – v);
}
void giro_eje_derecha(int v) {
servoDI.write(90 + v);
servoDD.write(90 + v);
servoTI.write(90 + v);
servoTD.write(90 + v);
}
long dist() {
long duracion, distancia;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duracion = pulseIn(echoPin, HIGH);
distancia = duracion * 0.017;
return distancia;
}