#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#define Trig 4
#define Echo 5
LiquidCrystal_I2C lcd(0x3F,16,2);
float Temp=30 ;
Servo myservo;
int angle_1 , angle_2;
float dist ,dist_1, dist_2 , dist_3,dist_4,dist_5;
float check(){
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
double t = pulseIn(Echo, HIGH,50000);
dist = (331+0.6*Temp)*t/20000 ;
if(dist<=3){
dist=3.00;
}else if(dist>200){
dist=200.00;
}
if(dist>3&&dist<500){
lcd.setCursor(0,0);
lcd.print(dist);
lcd.print("cm");
lcd.print(" ");
}else{
lcd.setCursor(0,0);
lcd.print("Out of Range...");
}
return dist; }
void search_aim(){
dist_1=199.00;
for(int i=0 ; i<180 ; i=i+5){
myservo.write(i);
do{
check();
delay(20);
}while(dist<4.00);
if(dist>199.00){
dist=200.00;
}
if(dist<dist_1){
dist_2 = dist;
dist_1=dist;
angle_2=i;
}
delay(10);
}
dist_1=199.00;
for(int i=180 ; i>0 ; i=i-5){
myservo.write(i);
do{
check();
delay(10);
}while(dist<4.00);
if(dist>199.00){
dist=200.00;
}
if(dist<dist_1){
dist_2 = dist;
dist_1=dist;
angle_2=i;
}
delay(10);
}
lcd.setCursor(0,1);
lcd.print("ang=");
lcd.print(angle_2);
lcd.print(" dis=");
lcd.print(dist_2);
}
void setup() {
Serial.begin(9600);
lcd.init();
lcd.backlight();
pinMode(Trig, OUTPUT);
myservo.attach(8);
search_aim();
}
void loop() {
myservo.write(angle_2);
}
第二版
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#define Trig 4
#define Echo 5
LiquidCrystal_I2C lcd(0x3F,16,2);
float Temp=30 ;
Servo myservo;
int angle_1 , angle_2, angle_3, angle_4,angle_new,angle_old,x=10,old;
float dist ,dist_1, dist_2 , dist_3,dist_4,dist_old,dist_new;
float track1,track2,track3,track4,track_old;
unsigned long t1,t2;
long t=20;
float measure(){
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
double t = pulseIn(Echo, HIGH,20000);
dist = (331+0.6*Temp)*t/20000 ;
if(dist<=3){
dist=3.00;
}else if(dist>200){
dist=200.00;
}
if(dist>3&&dist<500){
lcd.setCursor(0,0);
lcd.print(dist);
lcd.print("cm");
lcd.print(" ");
}else{
lcd.setCursor(0,0);
lcd.print("Out of Range...");
}
return dist; }
void search_aim(int mini , int maxi , int interval){
dist_1=199.00;
for(int i=mini ; i<maxi ; i=i+interval){
myservo.write(i);
do{delay(t);measure();}while(dist<4.00);
if(dist>199.00){dist=200.00;}
if(dist<dist_1){
dist_2 = dist;
dist_1=dist;
angle_2=i;
}}
/////////////////////////////////
dist_1=199.00;
for(int i=maxi ; i>mini ; i=i-interval){
myservo.write(i);
do{delay(t);measure();}while(dist<4.00);
if(dist>199.00){dist=200.00;}
if(dist<dist_1){
dist_3 = dist;
dist_1=dist;
angle_3=i;
}}
}
/////////////////////////////////////
void to_precise(int y,int d){
if(angle_3>=angle_2){
if(angle_2-y<=0){angle_2=0; }else{angle_2=angle_2-y;}
if(angle_3+y>=180){angle_3=180; }else{angle_3=angle_3+y;}
search_aim(angle_2,angle_3,d);
}else{
if(angle_3-y<=0){angle_3=0; }else{angle_3=angle_3-y;}
if(angle_2+y>=180){angle_2=180; }else{angle_2=angle_2+y;}
search_aim(angle_3,angle_2,d);
}}
void get_data(){
angle_4=(angle_2+angle_3)/2;
dist_4=(dist_2+dist_3)/2.00;
myservo.write(angle_4);
lcd.setCursor(0,1);
lcd.print("ang=");
lcd.print(angle_4);
lcd.print(" dis=");
lcd.print(dist_4);
}
////////////////////////////////////
void searching(){
search_aim(0 , 180 , 10);
to_precise(5,10);
to_precise(3,5);
to_precise(2,3);
to_precise(1,2);
get_data();
}
////////////////////////////////////
void track(){
for(int i=16 ; i>0 ; i=i-5){
to_precise(i,i);
get_data();
}
}
void setup() {
Serial.begin(9600);
lcd.init();
lcd.backlight();
pinMode(Trig, OUTPUT);
myservo.attach(8);
searching();
}
void loop() {
}
超音波感測器
Trig = 9
Echo = 8
Servo伺服馬達
pin 4