/*
 Controlling a servo position using a potentiometer (variable resistor)
 by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

 modified on 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Knob
*/

#include <Servo.h>

#include <Ultrasonic.h>
Ultrasonic ultrasonic(6, 5); // TRIG/ECHO

Servo myservo;  // create servo object to control a servo

long L_1 = 0 ;
long L_2 = 0 ;

int num_1 , num_servo = 90 ;
void setup() {
  Serial.begin(9600);
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  myservo.write(num_servo); 
  ultrasonic.setTimeout(40000UL);
}

void loop() {
Serial.print("Ultrasonic = ");
Serial.print(num_1);
Serial.print(" num_servo = ");
Serial.println(num_servo);
num_1 = map(ultrasonic.read() , 2,420,0,180);
SERVO_CONTRON (num_1) ;
}


void SERVO_CONTRON (int U) {

  if((U >= 90) && (U <= 110)){
    num_servo = 90 ;
  }
//---------------------------------

  if((U > 0) && (U <= 90)){
    if((millis () - L_1) >= 50){
      L_1 = millis();
     num_servo += 1 ;
    }
  }
//---------------------------------
  if((U > 110) && (U <= 180)){
      if((millis () - L_2) >= 50){
      L_2 = millis();
      num_servo -= 1 ;
    }
  }
  myservo.write(num_servo); 
  if(num_servo < 0){ num_servo = 0 ;}
  if(num_servo > 180){num_servo = 180 ;}

}