#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <NewPing.h>
#include <Servo.h>

LiquidCrystal_I2C lcd(0x27,16,2);//0x27   0x3F

#define TRIGGER_PIN  2  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     3  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 400 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.


Servo myservo;
int pos = 0;

String returntemp = "";           //存储返回值 
int g_UTL = 0;
int shezhi = 7;                   //预设距离
int fre;//set the variable to store the frequence value

/*printf格式化字符串初始化*/
int serial_putc( char c, struct __file * )
{
  Serial.write( c );
  return c;
}
void printf_begin(void)
{
  fdevopen( &serial_putc, 0 );
}

void setup()
{
  myservo.attach(9);
  
  lcd.init(); 
  lcd.backlight();
  Serial.begin(9600);	          //波特率9600 
  printf_begin();                 //初始化printf
}


void loop() 
{   
 TP1:   
    delay(100); 
    unsigned int uS = sonar.ping();    
    g_UTL =  uS / US_ROUNDTRIP_CM;
    if (g_UTL == 0) goto TP1;   //误判
    
    lcd.setCursor(0, 0);
    lcd.print("Tips    :");
    lcd.setCursor(0, 1);
    lcd.print("             ");
    lcd.setCursor(2, 1);
    lcd.print("Close to use  ");            //提示：靠近后使用


     if(g_UTL < shezhi)         //如果检测到的超声波距离值小于设定的超声波距离值
     {
   
      delay(1000);
      lcd.setCursor(0, 0);
      lcd.print("Tips    :");
      lcd.setCursor(0, 1);
      lcd.print("                ");
      lcd.setCursor(1, 1);
      lcd.print("Please use it");           //提示：请使用
      delay(2000);
      for (pos = 0; pos <= 180; pos += 1) {        //舵机旋转从0度-180度
      myservo.write(pos);              
      delay(10);                      
  
      }
      delay(1000);
      
      for (pos = 180; pos >= 0; pos -= 1) {        // 180度-0度
      myservo.write(pos);              
      delay(10);                       

      } 

   delay(2000); 
     }
}