自動避障小車

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