// Incluímos la librería para poder controlar el servo

#include <Servo.h>

// Declaramos la variable para controlar el servo

Servo servoMotor;

const int EchoPin = 5;

const int TriggerPin = 6;

const int LedPin = 13;

int b = 0;

double f = 0;

double a = -180;

double x = 0;

double y = 0;

double z = 0;

double result;

void setup() {

// Iniciamos el monitor serie para mostrar el resultado

Serial.begin(9600);

// Iniciamos el servo para que empiece a trabajar con el pin 9

servoMotor.attach(9);

Serial.begin(9600);

pinMode(LedPin, OUTPUT);

pinMode(TriggerPin, OUTPUT);

pinMode(EchoPin, INPUT);

void loop() {
 for(f=0; f<180; f=f+1){

servoMotor.write(f);

//delay(50);

int cm = ping(TriggerPin, EchoPin);

//Serial.print("Distancia: ");

//Serial.println(cm);

z = cm*10;

// -----------------------------------------------------------

result = cos ((f-180) * 3.14159265f / 180.0f);

Serial.print(f);

Serial.print('\t');

Serial.print(x = z*result);

Serial.print('\t');

result = sin ((f-180) * 3.14159265f / 180.0f);

Serial.println(y = z*result);

delay(0);

// -----------------------------------------------------------

//Serial.print("La Distancia maxima es: ");

//Serial.println(b);

delay(0000);

for(f=180;f>0;f=f-1){

servoMotor.write(f);

//delay(50);

int cm = ping(TriggerPin, EchoPin);

//Serial.print("Distancia: ");

//Serial.println(cm);
 z = cm*10;

// -----------------------------------------------------------

result = cos ((f-180) * 3.14159265f / 180.0f);

Serial.print(f);

Serial.print('\t');

Serial.print(x = z*result);

Serial.print('\t');

result = sin ((f-180) * 3.14159265f / 180.0f);

Serial.println(y = z*result);

delay(0);

// -----------------------------------------------------------

delay(0000);

int ping(int TriggerPin, int EchoPin) {

long duration, distanceCm;

digitalWrite(TriggerPin, LOW); //para generar un pulso limpio ponemos a LOW 4us

delayMicroseconds(4);

digitalWrite(TriggerPin, HIGH); //generamos Trigger (disparo) de 10us

delayMicroseconds(10);

digitalWrite(TriggerPin, LOW);

duration = pulseIn(EchoPin, HIGH); //medimos el tiempo entre pulsos, en microsegundos

distanceCm = duration * 10 / 292/ 2; //convertimos a distancia, en cm

return distanceCm;