// Includes the Servo library

#include <Servo.h>.

// Defines Tirg and Echo pins of the Ultrasonic Sensor

const int trigPin = 7;

const int echoPin = 8;

// Variables for the duration and the distance

long duracion;

int distancia;

int A;

// MOTOR

int motorA= 11;

int motorB= 10;

Servo servo; // Creates a servo object for controlling the servo motor

char BluetoothData;

char estado;

void setup() {

pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output

pinMode(echoPin, INPUT); // Sets the echoPin as an Input

Serial.begin(9600);

servo.attach(12); // Defines on which pin is the servo motor attached

pinMode (motorA,OUTPUT);//MOTOR L

pinMode (motorB,OUTPUT);//MOTOR R

void loop() {
 // rotates the servo motor from 15 to 165 degrees

for(int i=15;i<=165;i++){

if (Serial.available() >0) {

estado = Serial.read();

Serial.println(estado);

if(estado=='F')

A=255;

if(estado=='W'){

//if (contador<=44){

analogWrite(motorA,A);

analogWrite(motorB,A);

// delay(900);

} else {//si se comenta el else la rutina la hace continua

digitalWrite(motorA,LOW);

digitalWrite(motorB,LOW);

//delay(300);

// contador=0;

// estado='s';

if(estado=='S'){
 //carro quieto;

digitalWrite(motorA,LOW);

digitalWrite(motorB,LOW);

//IZQUIERDA

if(estado=='D'|| distancia<=30){

//if(contador==19){

digitalWrite(motorA,LOW);

analogWrite(motorB,A);

//delay(470);

//DERECHA

if(estado=='A'){

//if(contador==19){

analogWrite(motorA,A);

digitalWrite(motorB,LOW);

// delay(470);

servo.write(i);
 delay(20);

distancia = calcularDistancia();// Calls a function for calculating the distance measured by the
Ultrasonic sensor for each degree

Serial.print(i); // Sends the current degree into the Serial Port

Serial.print(","); // Sends addition character right next to the previous value needed later in the
Processing IDE for indexing

Serial.print(distancia); // Sends the distance value into the Serial Port

Serial.print("."); // Sends addition character right next to the previous value needed later in the
Processing IDE for indexing

// Repeats the previous lines from 165 to 15 degrees

for(int i=165;i>15;i--){

if (Serial.available() >0) {

estado = Serial.read();

Serial.println(estado);

if(estado=='F')

A=255;

if(estado=='W'){

//if (contador<=44){

analogWrite(motorA,A);

analogWrite(motorB,A);
// delay(900);

} else {//si se comenta el else la rutina la hace continua

digitalWrite(motorA,LOW);

digitalWrite(motorB,LOW);

//delay(300);

// contador=0;

// estado='s';

if(estado=='S'){

//carro quieto;

digitalWrite(motorA,LOW);

digitalWrite(motorB,LOW);

//IZQUIERDA

if(estado=='D' || distancia<=30){

//if(contador==19){

digitalWrite(motorA,LOW);

analogWrite(motorB,A);

//delay(470);

}
//DERECHA

if(estado=='A'){

//if(contador==19){

analogWrite(motorA,A);

digitalWrite(motorB,LOW);

// delay(470);

servo.write(i);

delay(20);

distancia = calcularDistancia();

Serial.print(i);

Serial.print(",");

Serial.print(distancia);

Serial.print(".");

// funcion para la distancia del Ultrasonido

int calcularDistancia(){

digitalWrite(trigPin, LOW);

delayMicroseconds(2);

// Sets the trigPin on HIGH state for 10 micro seconds

digitalWrite(trigPin, HIGH);

delayMicroseconds(10);
 digitalWrite(trigPin, LOW);

duracion = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound wave travel time in
microseconds

distancia= duracion*0.034/2;

return distancia;