#include <Servo.h>

Servo servo_6;
unsigned long task_time_ms=0;

unsigned long task_time_ms2=0;

double fnc_ultrasonic_distance(int _t, int _e){
	unsigned long dur=0;
	digitalWrite(_t, LOW);
	delayMicroseconds(5);
	digitalWrite(_t, HIGH);
	delayMicroseconds(10);
	digitalWrite(_t, LOW);
	dur = pulseIn(_e, HIGH, 18000);
	if(dur==0)return 999.0;
	return (dur/57);
}

void fnc_dynamic_analogWrite(int _pin, int _e){
	pinMode(_pin,OUTPUT);
	analogWrite(_pin,_e);
}

void setup()
{
  	pinMode(13, INPUT);
	servo_6.attach(6);
	pinMode(13, OUTPUT);
	pinMode(2, OUTPUT);
	pinMode(3, INPUT);

}


void loop()
{

  	if((millis()-task_time_ms)>=1000){
  		task_time_ms=millis();
  		if ((digitalRead(13) == true)) {
  			servo_6.write(120);
  			digitalWrite(13, LOW);
  		}
  		else {
  			servo_6.write(60);
  			digitalWrite(13, HIGH);
  		}

  	}
  	if((millis()-task_time_ms2)>=100){
  		task_time_ms2=millis();
  		if ((fnc_ultrasonic_distance(2,3) <= 15)) {
  			fnc_dynamic_analogWrite(9, 255);
  			fnc_dynamic_analogWrite(10, 0);
  			fnc_dynamic_analogWrite(11, 0);
  		}
  		else {
  			fnc_dynamic_analogWrite(9, 0);
  			fnc_dynamic_analogWrite(10, 0);
  			fnc_dynamic_analogWrite(11, 255);
  		}

  	}

}