This is an old revision of the document!
/*
* Prueba de robot R2D2
* Copyright 216 Leonardo Yamasaki Maza
* Licencia GPL.
*/
// General defines
#define MOTOR1P 2 //Pin Motor 1 positivo
#define MOTOR1N 3 //Pin Motor 1 negativo
#define MOTOR2P 4 //Pin Motor 2 positivo
#define MOTOR2N 5 //Pin Motor 2 negativo
#define SERVO 6 //Pin Señal del servo cable amarilo
#define ECHOPIN 7 //Ultrasonido
#define TRIGPIN 8 //Ultrasonido
#define RXBLUE 11 //Pin conectar a RX del modulo Bluetooth
#define TXBLUE 12 //Pin conectar a TX del modulo Bluetooth
#define TESTLED 13 //Pin del LED interno
#define FLASHTIME 100 //Tiempo de flasheo LED13
#define DELAYHEAD 15 //Retardo de posición del servo en ms
#define MAXSPEED 80 //Maxima velocidad
//Settings para servo mueve cabeza
#include <Servo.h>
Servo head;
int pos = 0;
//Variables de control
int espera; //espera
char cmd; //Comando enviado
int vel=MAXSPEED; //Velocidad de motores
//Comunicacion con bluetooth
#include <SoftwareSerial.h>
SoftwareSerial softSerial(RXBLUE, TXBLUE); // RX, TX Bluetooth
// Movimiento del servo para cabeza
void headmove(char c) {
if(c=='d') {
for (pos = 90; pos >= 0; pos -= 1) {
head.write(pos);
delay(DELAYHEAD);
}
for (pos = 0; pos <= 90; pos += 1) {
head.write(pos);
delay(DELAYHEAD);
}
}
if(c=='i') {
for (pos = 90; pos <= 180; pos += 1) {
head.write(pos);
delay(DELAYHEAD);
}
for (pos = 180; pos >= 90; pos -= 1) {
head.write(pos);
delay(DELAYHEAD);
}
}
if(c=='c') {
pos = 90;
head.write(pos);
delay(DELAYHEAD);
}
if(c=='m') {
for (pos = 90; pos >= 0; pos -= 1) {
head.write(pos);
delay(DELAYHEAD);
}
for (pos = 0; pos <= 90; pos += 1) {
head.write(pos);
delay(DELAYHEAD);
}
for (pos = 90; pos <= 180; pos += 1) {
head.write(pos);
delay(DELAYHEAD);
}
for (pos = 180; pos >= 90; pos -= 1) {
head.write(pos);
delay(DELAYHEAD);
}
}
}
//
// Control de motores
//
void parar(){
digitalWrite(MOTOR1P,LOW);
digitalWrite(MOTOR1N,LOW);
digitalWrite(MOTOR2P,LOW);
digitalWrite(MOTOR2N,LOW);
}
void adelante(){
digitalWrite(MOTOR1P,LOW);
analogWrite(MOTOR1N,vel);
digitalWrite(MOTOR2P,HIGH);
analogWrite(MOTOR2N,256-vel);
}
void atras(){
digitalWrite(MOTOR1P,HIGH);
analogWrite(MOTOR1N,256-vel);
digitalWrite(MOTOR2P,LOW);
analogWrite(MOTOR2N,vel);
}
void derecha(){
digitalWrite(MOTOR1P,HIGH);
analogWrite(MOTOR1N,256-vel);
digitalWrite(MOTOR2P,HIGH);
analogWrite(MOTOR2N,256-vel);
}
void izquierda(){
digitalWrite(MOTOR1P,LOW);
analogWrite(MOTOR1N,vel);
digitalWrite(MOTOR2P,LOW);
analogWrite(MOTOR2N,vel);
}
void derechaLenta(){
digitalWrite(MOTOR1P,LOW);
digitalWrite(MOTOR1N,LOW);
digitalWrite(MOTOR2P,HIGH);
analogWrite(MOTOR2N,256-vel);
}
void izquierdaLenta(){
digitalWrite(MOTOR1P,LOW);
analogWrite(MOTOR1N,vel);
digitalWrite(MOTOR2P,LOW);
digitalWrite(MOTOR2N,LOW);
}
//Flashe LED 13 in borad
void flash(int p, int t){
digitalWrite(p,HIGH);
delay(t);
digitalWrite(p,LOW);
delay(t);
}
//Ultrasonido
int ultrasonido(){
int maximumRange = 200; // Maximum range needed
int minimumRange = 0; // Minimum range needed
long duration, distance; // Duration used to calculate distance
/* The following trigPin/echoPin cycle is used to determine the
distance of the nearest object by bouncing soundwaves off of it. */
digitalWrite(TRIGPIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGPIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGPIN, LOW);
duration = pulseIn(ECHOPIN, HIGH);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
if (distance >= maximumRange || distance < = minimumRange){
/* Send a negative number to computer and Turn LED ON to indicate "out of range" */
// Serial.println("No reading");
digitalWrite(TESTLED, HIGH);
}
else {
/* Send the distance to the computer using Serial protocol, and
turn LED OFF to indicate successful reading. */
// Serial.println(distance);
digitalWrite(TESTLED, LOW);
}
//Delay 50ms before next reading.
delay(50);
return distance;
}
///////////////
/*
* Setup y loop
*
*/
void setup() {
//Motores puente H
pinMode(MOTOR1P,OUTPUT);
pinMode(MOTOR1N,OUTPUT);
pinMode(MOTOR2P,OUTPUT);
pinMode(MOTOR2N,OUTPUT);
pinMode(TESTLED,OUTPUT);
//Ultrasonido
pinMode(TRIGPIN, OUTPUT);
pinMode(ECHOPIN, INPUT);
//Servo config
head.attach(SERVO);
headmove('c');
// set the data rate for the SoftwareSerial port
softSerial.begin(9600);
softSerial.println("Hello, R2D2!\r\n");
// Open serial communications and wait for port to open:
// Serial.begin(9600);
// while (!Serial) {
// ;
// }
// Serial.println('2nd Hello R2D2!\r\n');
}
void loop() {
if (softSerial.available()) {
cmd = softSerial.read();
softSerial.write(cmd);
softSerial.write(':');
}
switch(cmd){
case 'f':
adelante();
softSerial.println("Adelante");
espera=1;
break;
case 'b':
atras();
softSerial.println("Atras");
espera=1;
break;
case 'r':
derecha();
Serial.println("Derecha");
espera=1;
break;
case 'l':
izquierda();
softSerial.println("Izquierda");
espera=1;
break;
case 'd':
derechaLenta();
Serial.println("Derecha lenta");
espera=1;
break;
case 'i':
izquierdaLenta();
softSerial.println("Izquierda lenta");
espera=1;
break;
case 's':
parar();
headmove('c');
softSerial.println("Parar");
espera=1;
break;
case '+':
softSerial.println("Sube velocidad");
espera=1;
vel+=20;
if(vel>256){
vel=255;
softSerial.println("Limite sube velocidad");
}
break;
case '-':
softSerial.println("Baja velocidad");
espera=1;
vel-=20;
if(vel<80){
vel=80;
softSerial.println("Limite baja velocidad");
}
break;
case 'm':
softSerial.println("Cabeza escaneando");
espera=1;
headmove('m');
break;
case 'n':
softSerial.println("Cabeza izquierda");
espera=1;
headmove('i');
break;
case 'o':
softSerial.println("Cabeza derecha");
espera=1;
headmove('d');
break;
case 'u':
softSerial.println("Distancia ultrasonido:");
softSerial.print(ultrasonido());
break;
default:
if(espera) {
softSerial.println("En espera");
espera=0;
flash(TESTLED,FLASHTIME);
softSerial.println("Distancia ultrasonido:");
softSerial.print(ultrasonido());
}
break;
} //End switch
cmd='.';
}
//END