自动避障小车

#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;
}