//APSC 101-last update 2016 Jan

#include <Servo.h>
#define SERVO_PIN 9
#define GROUND_JOY_PIN A3 //joystick ground pin will connect to Arduino
analog pin A3
#define VOUT_JOY_PIN A2 //joystick +5 V pin will connect to Arduino
analog pin A2
#define XJOY_PIN A1 //X axis reading from joystick will go into
analog pin A1
Servo myservo ;
const int LedPinVerde = 12;
const int LedPinYellow = 11;
const int LedPinRojo = 3;
const int trigPin = 6;
const int echoPin = 10;

// defines variables
long duration;
int distance;

void setup()
{
Serial.begin(9600);
pinMode(VOUT_JOY_PIN, OUTPUT) ; //pin A3 shall be used as output
pinMode(GROUND_JOY_PIN, OUTPUT) ; //pin A2 shall be used as output
digitalWrite(VOUT_JOY_PIN, HIGH) ; //set pin A3 to high (+5V)
digitalWrite(GROUND_JOY_PIN,LOW) ; //set pin A3 to low (ground)
myservo.attach(9);
pinMode(LedPinVerde, OUTPUT);
pinMode(LedPinYellow, OUTPUT);
pinMode(LedPinRojo, OUTPUT);
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
}

void loop()
{
delay(200);
int joystickXVal = analogRead(XJOY_PIN) ; //read joystick input on pin A1
Serial.print(joystickXVal); //print the value from A1
Serial.println(" = input from joystick"); //print "=input from joystick" next to
the value
Serial.print((joystickXVal+520)/10); //print a from A1 calculated, scaled
value
Serial.println(" = output to servo"); //print "=output to servo" next to the
value
Serial.println() ;
myservo.write((joystickXVal+520)/10); //write the calculated value to the
servo
// Clears the trigPin
digitalWrite(trigPin, LOW);
delayMicroseconds(2);

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

digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);

// Calculating the distance

distance= duration*0.034/2;

// Prints the distance on the Serial Monitor

Serial.print("Distance: ");
Serial.println(distance);
if (distance >= 20) {
digitalWrite(LedPinVerde , HIGH);
digitalWrite(LedPinYellow , LOW);
digitalWrite(LedPinRojo , LOW);
}
else if((distance <= 19) && (distance >= 10)){
digitalWrite(LedPinVerde , LOW);
digitalWrite(LedPinRojo , LOW);
digitalWrite(LedPinYellow , HIGH);

}
else if (distance <=9) {
digitalWrite(LedPinVerde , LOW);
digitalWrite(LedPinYellow , LOW);
digitalWrite(LedPinRojo , HIGH);
}

delay(10);