[[examplescode|Regresa a codigos]]
<sxh c> 
/*
 * Prueba de robot R2D2
 * Copyright Octubre 2019 Leonardo Yamasaki Maza
 * Licencia GPL.
 */

#include <SoftwareSerial.h>
#include <Servo.h>

// 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 SERVO1     6  //Pin Señal del servo cable amarilo
#define SERVO2     7  //Pin Señal del servo cable amarilo
#define SERVO3     8  //Pin Señal del servo cable amarilo
#define HEADPIN    8
#define ECHOPIN    9  //Ultrasonido
#define TRIGPIN   10  //Ultrasonido
#define RXBLUE    11  //Pin conectar a RX del modulo Bluetooth
#define TXBLUE    12  //Pin conectar a TX del modulo Bluetooth
#define TESTLED   7  //Pin del LED interno
#define FLASHTIME 50s  //Tiempo de flasheo LED13
#define DELAYHEAD 30  //Retardo de posición del servo en ms
#define MAXSPEED  80  //Maxima velocidad

//Settings para servo mueve cabeza
Servo head;
int pos = 0;

//Variables de control
int espera; //espera
char cmd; //Comando enviado
int vel=MAXSPEED;  //Velocidad de motores

//Comunicacion con bluetooth
SoftwareSerial softSerial(RXBLUE, TXBLUE); // RX, TX Bluetooth

// Movimiento del servo para cabeza
void headmove(char c) {
  head.attach(HEADPIN);
  if(c=='d') {
    for (pos = 90; pos >= 0; pos -= 5) {
      head.write(pos);
      delay(DELAYHEAD);
    }
    for (pos = 0; pos <= 90; pos += 5) {
      head.write(pos);
      delay(DELAYHEAD);
    }
  }
  if(c=='i') {
    for (pos = 90; pos <= 180; pos += 5) {
      head.write(pos);
      delay(DELAYHEAD);
    }
    for (pos = 180; pos >= 90; pos -= 5) {
      head.write(pos);
      delay(DELAYHEAD);
    }
  }
  if(c=='c') {
    pos = 90;
    head.write(pos);
    delay(3000);
  }
  if(c=='m') {
    for (pos = 90; pos >= 0; pos -= 5) {
      head.write(pos);
      delay(DELAYHEAD);
    }
    for (pos = 0; pos <= 90; pos += 5) {
      head.write(pos);
      delay(DELAYHEAD);
    }
    for (pos = 90; pos <= 180; pos += 5) {
      head.write(pos);
      delay(DELAYHEAD);
    }
    for (pos = 180; pos >= 90; pos -= 5) {
      head.write(pos);
      delay(DELAYHEAD);
    }
  }
  head.detach();
}
//
// 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 TESTLED in board
void flash(int p, int t){
  digitalWrite(p,LOW);
  delay(t);
  digitalWrite(p,HIGH);
  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() {
  digitalWrite(TESTLED,HIGH);
  //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(HEADPIN);
  headmove('c');
  head.detach();

  // 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("Hello R2D2!\r\n");
}
void loop() {
  if (softSerial.available()) {
    cmd = softSerial.read();
    softSerial.write(cmd);
    softSerial.write(':');
  }
//  digitalWrite(TESTLED,LOW);
//  delay(50);
//  digitalWrite(TESTLED,HIGH);
//  delay(50);
  switch(cmd){
    case 'f':
      adelante();
      softSerial.println("Adelante");
      Serial.println("Adelante");
      espera=1;
      break;
    case 'b':
      atras();
      softSerial.println("Atras");
      Serial.println("Atras");
      espera=1;
      break;
    case 'r':
      derecha();
      softSerial.println("Derecha");
      Serial.println("Derecha");
      espera=1;
      break;
    case 'l':
      izquierda();
      softSerial.println("Izquierda");
      Serial.println("Izquierda");
      espera=1;
      break;
    case 'd':
      derechaLenta();
      softSerial.println("Derecha lenta");
      Serial.println("Derecha lenta");
      espera=1;
      break;
    case 'i':
      izquierdaLenta();
      softSerial.println("Izquierda lenta");
      Serial.println("Izquierda lenta");
      espera=1;
      break;
    case 's':
      parar();
      softSerial.println("Parar");
      Serial.println("Parar");
      espera=1;
      break;
    case '+':
      softSerial.println("Sube velocidad");
      Serial.println("Sube velocidad");
      espera=1;
      vel+=20;
      if(vel>256){
        vel=255;
        softSerial.println("Limite sube velocidad");
        Serial.println("Limite sube velocidad");
      }
      break;
    case '-':
      softSerial.println("Baja velocidad");
      Serial.println("Baja velocidad");
      espera=1;
      vel-=20;
      if(vel<80){
        vel=80;
        softSerial.println("Limite baja velocidad");
        Serial.println("Limite baja velocidad");
      }
      break;
    case 'm':
      softSerial.println("Cabeza escaneando");
      Serial.println("Cabeza escaneando");
      espera=1;
      headmove('m');
      break;
    case 'n':
      softSerial.println("Cabeza izquierda");
      Serial.println("Cabeza izquierda");
      espera=1;
      headmove('i');
      break;
    case 'o':
      softSerial.println("Cabeza derecha");
      Serial.println("Cabeza derecha");
      espera=1;
      headmove('d');
      break;
    case 'u':
      softSerial.println("Distancia ultrasonido:");
      Serial.print(ultrasonido());
      Serial.println("Distancia ultrasonido:");
      break;
    default:
      if(espera) {
        Serial.println("En espera");
        espera=0;
        flash(TESTLED,FLASHTIME);
        //softSerial.println("Distancia ultrasonido:");
        //softSerial.print(ultrasonido());
      }
      break;
  } //End switch
  cmd='.';
}
//END
</sxh>