#include <Servo.h>//include Servo library
Servo panMotor;
int left1=4;
int left2=5;
int right1=6;
int right2=7;
const int TrigPin = 2;
const int EchoPin = 3;
float cm;
float cm_1;
float cm_2;
int ledLeftpin=9;
int ledRightpin=10;
//
int photocellPin = 1; //定义变量photocellsh=2,为电压读取端口。
int ledPin = 12; //定义变量ledPin=12,为led电平输出端口
int val = 0; //定义val变量的起始值
//
int servopin = 8; //定义舵机接口数字接口7
void servopulse(int angle)//定义一个脉冲函数
{
int pulsewidth=(angle*11)+500; //将角度转化为500-2480的脉宽值
digitalWrite(servopin,HIGH); //将舵机接口电平至高
delayMicroseconds(pulsewidth); //延时脉宽值的微秒数
digitalWrite(servopin,LOW); //将舵机接口电平至低
delayMicroseconds(20000-pulsewidth);
}
void setup(){
Serial.begin(9600);
pinMode(ledPin, OUTPUT); //使ledPin为输出模式
pinMode(TrigPin, OUTPUT);
pinMode(EchoPin, INPUT);
pinMode(ledLeftpin,OUTPUT);
pinMode(ledRightpin,OUTPUT);
panMotor.attach(8); //将电机安装在适当管脚上
panMotor.write(90); //将PING)))焊盘设为中央
for (int pinindex = 4; pinindex <=7; pinindex++) {
pinMode(pinindex, OUTPUT); // set pins 3 to 10 as outputs
}
}
void loop()
{
cm=ping();
Serial.print(cm);
Serial.print("cm");
Serial.println();
val = analogRead(photocellPin); //从传感器读取值
if(val<=512){ //512=2.5V,想让传感器敏感一些的时候,把数值调高,想让传感器迟钝的时候把数值调低。
digitalWrite(ledPin, HIGH); //当val小于512(2.5V)的时候,led亮。
}
else{
digitalWrite(ledPin, LOW);
}
if(cm<=50.0)
{
digitalWrite(left1,LOW);
digitalWrite(left2, LOW);
digitalWrite(right1, LOW);
digitalWrite(right2, LOW); //stop
delay(1000);
panMotor.write(0);
delay(500);
cm_1=ping();
delay(500);
panMotor.write(180);
delay(700);
cm_2=ping();
delay(500);
panMotor.write(90); //回到中央
delay(1000);
delay(500);
if(cm_1<=20.0&&cm_2<=20.0) //backward
{
digitalWrite(ledLeftpin,HIGH);
digitalWrite(ledRightpin,HIGH);
digitalWrite(left1, HIGH);
digitalWrite(left2, LOW);
digitalWrite(right1, HIGH);
digitalWrite(right2, LOW);
delay(2000);
digitalWrite(ledLeftpin,LOW);
digitalWrite(ledRightpin,LOW);
}
else
{
if(cm_1>cm_2){
digitalWrite(ledRightpin,HIGH);
digitalWrite(left1,HIGH);
digitalWrite(left2, LOW);
digitalWrite(right1, LOW);
digitalWrite(right2, HIGH);
delay(2000);
digitalWrite(ledRightpin,LOW);
}
else
{
digitalWrite(ledLeftpin,HIGH);
digitalWrite(left1,LOW);
digitalWrite(left2, HIGH);
digitalWrite(right1, HIGH);
digitalWrite(right2, LOW);
delay(2000);
digitalWrite(ledLeftpin,LOW);
}
}
}
else
{
digitalWrite(left1, LOW);
digitalWrite(left2, HIGH);
digitalWrite(right1, LOW);
digitalWrite(right2, HIGH);
}
}
float ping()
{
float c;
digitalWrite(TrigPin, LOW); //低高低电平发一个短时间脉冲去TrigPin
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
c = pulseIn(EchoPin, HIGH) / 58.0; //将回波时间换算成cm
c = (int(c * 100.0)) / 100.0; //保留两位小数
return c;
}
自动避障小车
最新推荐文章于 2022-09-25 12:38:37 发布