373 lines
10 KiB
C++
373 lines
10 KiB
C++
// Adafruit Motor shield library
|
|
// copyright Adafruit Industries LLC, 2009
|
|
// this code is public domain, enjoy!
|
|
|
|
#include <AFMotor.h>
|
|
#include <SoftwareSerial.h>
|
|
#include <Servo.h>
|
|
|
|
AF_DCMotor motor1(2);
|
|
AF_DCMotor motor2(1);
|
|
AF_DCMotor motor3(3);
|
|
AF_DCMotor motor4(4);
|
|
|
|
volatile int contador = 0; // Declaramos como 'volatile' las variables que participan dentro y fuera de la interrupción.
|
|
int n = 0; // Variable auxiliar para notar cambios en el contador.
|
|
int vueltas = 0;
|
|
//int paso = 128; // Flancos de subida y bajada necesarios para que el encoder de una vuelta completa.
|
|
//int paso = 64; // Flancos de subidoa o bajada necesarios para que el encoder de una vuelta completa.
|
|
int paso = 32; // Cantidad de dientes. ¿Dará solo una vuelta por fin?
|
|
int pasosvuelta = 42; // Pasos del engranaje grande (blanco) para dar una vuelta completa.
|
|
int vueltacompleta = paso * pasosvuelta; // Cuenta para detectar vuelta completa.
|
|
int girarcuello = vueltacompleta;
|
|
|
|
int luztrasera = 13;
|
|
int luzdelantera = A1;
|
|
int mibocina = A0;
|
|
|
|
Servo servo1;
|
|
Servo servo2;
|
|
int angservo1;
|
|
int angservo2;
|
|
|
|
boolean buzzer = false;
|
|
boolean motores = false;
|
|
|
|
SoftwareSerial BT1(0, 1); // RX | TX
|
|
|
|
char comando = 'S';
|
|
char prevComando = 'A';
|
|
int velocidad = 255;
|
|
unsigned long timer0 = 2000; //Stores the time (in millis since execution started)
|
|
unsigned long timer1 = 0; //Stores the time when the last command was received from the phone
|
|
//int val;
|
|
int minimo = 650;
|
|
int maximo = 2550;
|
|
|
|
void setup() {
|
|
// Serial.begin(9600); // set up Serial library at 9600 bps
|
|
pinMode(mibocina, OUTPUT); // Bocina
|
|
pinMode(luzdelantera, OUTPUT); // Luz delantera
|
|
pinMode(luztrasera, OUTPUT); // Luz trasera
|
|
|
|
servo1.attach(9, minimo, maximo); // 1000 y 2000 son valores para el SG90
|
|
servo2.attach(10, minimo, maximo);
|
|
angservo1 = maximo;
|
|
angservo2 = 0;
|
|
servo1.write(angservo1);
|
|
servo2.write(angservo2);
|
|
|
|
// turn on motor
|
|
motor1.setSpeed(velocidad); // Rueda
|
|
motor2.setSpeed(velocidad); // Rueda
|
|
motor3.setSpeed(velocidad); // Cuello
|
|
motor4.setSpeed(velocidad); // Pinza
|
|
|
|
motor1.run(RELEASE);
|
|
motor2.run(RELEASE);
|
|
motor3.run(RELEASE);
|
|
motor4.run(RELEASE);
|
|
|
|
BT1.begin(9600);
|
|
|
|
attachInterrupt(0, EncoderCuello, RISING); // Sensor del encoder del motor de la cabeza.
|
|
|
|
}
|
|
|
|
void loop() {
|
|
if (BT1.available()) { // Usar si control por bluetooth.
|
|
// if(Serial.available()>0) { // Usar si control por puerto serie.
|
|
timer1 = millis();
|
|
prevComando = comando;
|
|
comando = BT1.read();
|
|
// Serial.write(comando);
|
|
// comando = Serial.read(); // Usar si control por puerto serie.
|
|
if (comando != prevComando) {
|
|
switch (comando) {
|
|
|
|
// Código para los servos. Minúsculas para servo 1 y mayúsuculas para servo2
|
|
case 'a':
|
|
servo1.writeMicroseconds(minimo);
|
|
angservo1 = minimo;
|
|
break;
|
|
case 'c':
|
|
servo1.write(minimo+(maximo - minimo)/4);
|
|
angservo1 = minimo+(maximo - minimo)/4;
|
|
break;
|
|
case 'e':
|
|
servo1.writeMicroseconds(minimo+(maximo - minimo)/2);
|
|
angservo1 = minimo+(maximo - minimo)/2;
|
|
break;
|
|
case 'p':
|
|
servo1.write(minimo+(maximo - minimo)*3/4);
|
|
angservo1 = minimo+(maximo - minimo)*3/4;
|
|
break;
|
|
case 'z':
|
|
servo1.writeMicroseconds(maximo);
|
|
angservo1 = maximo;
|
|
break;
|
|
case 'A':
|
|
servo2.write(minimo);
|
|
angservo2 = minimo;
|
|
break;
|
|
case 'C':
|
|
servo2.write(minimo+(maximo - minimo)/4);
|
|
angservo2 = minimo+(maximo - minimo)/4;
|
|
break;
|
|
case 'E':
|
|
servo2.write(minimo+(maximo - minimo)/2);
|
|
angservo2 = minimo+(maximo - minimo)/2;
|
|
break;
|
|
case 'P':
|
|
servo2.write(minimo+(maximo - minimo)*3/4);
|
|
angservo2 = minimo+(maximo - minimo)*3/4;
|
|
break;
|
|
case 'Z':
|
|
servo2.write(maximo);
|
|
angservo2 = maximo;
|
|
break;
|
|
case 't':
|
|
angservo1 = angservo1 + (maximo - minimo)/180;
|
|
if (angservo1 > maximo) {
|
|
angservo1 = maximo;
|
|
};
|
|
servo1.write(angservo1);
|
|
break;
|
|
case 'T':
|
|
angservo2 = angservo2 + (maximo - minimo)/180;
|
|
if (angservo2 > maximo) {
|
|
angservo2 = maximo;
|
|
};
|
|
servo2.write(angservo2);
|
|
break;
|
|
case 'y':
|
|
angservo1 = angservo1 + (maximo - minimo)/18;
|
|
if (angservo1 > maximo) {
|
|
angservo1 = maximo;
|
|
};
|
|
servo1.write(angservo1);
|
|
break;
|
|
case 'Y':
|
|
angservo2 = angservo2 + (maximo - minimo)/18;
|
|
if (angservo2 > maximo) {
|
|
angservo2 = maximo;
|
|
};
|
|
servo2.write(angservo2);
|
|
break;
|
|
case 'k':
|
|
angservo1 = angservo1 + (maximo - minimo)/4;
|
|
if (angservo1 > maximo) {
|
|
angservo1 = maximo;
|
|
};
|
|
servo1.write(angservo1);
|
|
break;
|
|
case 'K':
|
|
angservo2 = angservo2 + (maximo - minimo)/4;
|
|
if (angservo2 > maximo) {
|
|
angservo2 = maximo;
|
|
};
|
|
servo2.write(angservo2);
|
|
break;
|
|
case 'm':
|
|
angservo1 = angservo1 -(maximo - minimo)/180;
|
|
if (angservo1 < minimo) {
|
|
angservo1 = minimo;
|
|
};
|
|
servo1.write(angservo1);
|
|
break;
|
|
case 'M':
|
|
angservo2 = angservo2 - (maximo - minimo)/180;
|
|
if (angservo2 < minimo) {
|
|
angservo2 = minimo;
|
|
};
|
|
servo2.write(angservo2);
|
|
break;
|
|
case 'n':
|
|
angservo1 = angservo1 - (maximo - minimo)/18;
|
|
if (angservo1 < minimo) {
|
|
angservo1 = minimo;
|
|
};
|
|
servo1.write(angservo1);
|
|
break;
|
|
case 'N':
|
|
angservo2 = angservo2 - (maximo - minimo)/18;
|
|
if (angservo2 < minimo) {
|
|
angservo2 = minimo;
|
|
};
|
|
servo2.write(angservo2);
|
|
break;
|
|
case 'o':
|
|
angservo1 = angservo1 - (maximo - minimo)/4;
|
|
if (angservo1 < minimo) {
|
|
angservo1 = minimo;
|
|
};
|
|
servo1.write(angservo1);
|
|
break;
|
|
case 'O':
|
|
angservo2 = angservo2 - (maximo - minimo)/4;
|
|
if (angservo2 < minimo) {
|
|
angservo2 = minimo;
|
|
};
|
|
servo2.write(angservo2);
|
|
break;
|
|
|
|
|
|
// Código para bocina.
|
|
case 'V':
|
|
buzzer = true;
|
|
break;
|
|
case 'v':
|
|
buzzer = false;
|
|
break;
|
|
// Código para motores.
|
|
case 'F':
|
|
if (motores) {
|
|
motor3.run(FORWARD);
|
|
}
|
|
else {
|
|
motor1.run(FORWARD);
|
|
motor2.run(FORWARD);
|
|
};
|
|
break;
|
|
case 'f':
|
|
motor3.run(FORWARD);
|
|
break;
|
|
case 'B':
|
|
if (motores) {
|
|
motor3.run(BACKWARD);
|
|
} else {
|
|
motor1.run(BACKWARD);
|
|
motor2.run(BACKWARD);
|
|
};
|
|
break;
|
|
case 'b':
|
|
motor3.run(BACKWARD);
|
|
break;
|
|
case 'L':
|
|
if (motores) {
|
|
motor4.run(FORWARD);
|
|
} else {
|
|
motor2.run(FORWARD);
|
|
motor1.run(BACKWARD);
|
|
};
|
|
break;
|
|
case 'l':
|
|
motor4.run(FORWARD);
|
|
break;
|
|
case 'R':
|
|
if (motores) {
|
|
motor4.run(BACKWARD);
|
|
} else {
|
|
motor1.run(FORWARD);
|
|
motor2.run(BACKWARD);
|
|
};
|
|
break;
|
|
case 'r':
|
|
motor4.run(BACKWARD);
|
|
break;
|
|
case 'S': // Solo detiene ruedas
|
|
motor2.run(RELEASE);
|
|
motor1.run(RELEASE);
|
|
break;
|
|
case 'D': // Detiene todos los motores
|
|
motor2.run(RELEASE);
|
|
motor1.run(RELEASE);
|
|
motor3.run(RELEASE);
|
|
motor4.run(RELEASE);
|
|
break;
|
|
// Código para luces.
|
|
case 'W':
|
|
digitalWrite(luzdelantera, HIGH);
|
|
break;
|
|
case 'w':
|
|
digitalWrite(luzdelantera, LOW);
|
|
break;
|
|
case 'U':
|
|
digitalWrite(luztrasera, HIGH);
|
|
break;
|
|
case 'u':
|
|
digitalWrite(luztrasera, LOW);
|
|
break;
|
|
// Códigos para cuello.
|
|
case 'Q':
|
|
contador = 0;
|
|
break;
|
|
case 'x':
|
|
motores = false;
|
|
break;
|
|
case 'X':
|
|
motores = true;
|
|
break;
|
|
case 'q':
|
|
girarcuello = vueltacompleta;
|
|
contador = 0;
|
|
break;
|
|
case '9':
|
|
girarcuello = vueltacompleta;
|
|
contador = 0;
|
|
break;
|
|
case '8':
|
|
girarcuello = vueltacompleta / 2;
|
|
contador = 0;
|
|
break;
|
|
case '7':
|
|
girarcuello = vueltacompleta / 2;
|
|
contador = 0;
|
|
break;
|
|
case '6':
|
|
girarcuello = vueltacompleta / 3;
|
|
contador = 0;
|
|
break;
|
|
case '5':
|
|
girarcuello = vueltacompleta / 3;
|
|
contador = 0;
|
|
break;
|
|
case '4':
|
|
girarcuello = vueltacompleta / 4;
|
|
contador = 0;
|
|
break;
|
|
case '3':
|
|
girarcuello = vueltacompleta / 4;
|
|
contador = 0;
|
|
break;
|
|
case '2':
|
|
girarcuello = 32;
|
|
contador = 0;
|
|
break;
|
|
case '1':
|
|
girarcuello = 32;
|
|
contador = 0;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
timer0 = millis(); //Get the current time (millis since execution started).
|
|
//Check if it has been 500ms since we received last command.
|
|
if ((timer0 - timer1) > 500) {
|
|
//More tan 500ms have passed since last command received, car is out of range.
|
|
//Therefore stop the car and turn lights off.
|
|
motor2.run(RELEASE);
|
|
motor1.run(RELEASE);
|
|
}
|
|
}
|
|
|
|
if (buzzer) {
|
|
if ((millis() % 2) == 1) {
|
|
digitalWrite(mibocina, HIGH);
|
|
}
|
|
else {
|
|
digitalWrite(mibocina, LOW);
|
|
}
|
|
}
|
|
if (contador > girarcuello) {
|
|
contador = 0;
|
|
motor3.run(RELEASE);
|
|
vueltas++;
|
|
}
|
|
}
|
|
|
|
void EncoderCuello() {
|
|
contador++;
|
|
}
|