This is an old revision of the document!
/* * Prueba de robot R2D2 * © 2016 Copyright 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