ARDUINO小车避障程序

//PASS OK AT UNO+L298,20130518,23:18
#define DEBUG 0    // set to 1 to print to serial monitor, 0 to disable
#include <Servo.h>
Servo headservo;  // 头部舵机对象
// Constants
const int EchoPin = 6; //超声波信号输入
const int TrigPin = 5; //超声波控制信号输出
const int leftmotorpin1 = 12; // 直流电机信号输出
const int leftmotorpin2 = 3;
const int rightmotorpin1 = 13;
const int rightmotorpin2 = 11;
const int HeadServopin = 2; // 舵机信号输出 只有9或10接口可利用
const int Sharppin = 3; // 红外输入
const int maxStart = 800;  //run dec time
// Variables
int isStart = maxStart;    //启动
int currDist = 0;    // 距离
boolean running = false;
void setup() {
  Serial.begin(9600); // 开始串行监测
  //信号输入接口
  pinMode(EchoPin, INPUT);
  pinMode(TrigPin, OUTPUT);
  pinMode(Sharppin, INPUT);
  pinMode(HeadServopin, OUTPUT);
  pinMode(leftmotorpin1, OUTPUT);
  pinMode(leftmotorpin2, OUTPUT);
  pinMode(rightmotorpin1, OUTPUT);
  pinMode(rightmotorpin1, OUTPUT);
  //信号输出接口
  //for (int pinindex = 3; pinindex < 11; pinindex++) {
   // pinMode(pinindex, OUTPUT); // set pins 3 to 10 as outputs
  //}
  //舵机接口
  headservo.attach(HeadServopin);
  //启动缓冲活动头部
  headservo.write(70);
  delay(2000);
  headservo.write(20);
  delay(2000);
}
void loop() {
  if(DEBUG){
    Serial.print("running:");
    if(running){
      Serial.println("true");
    }
    else{
      Serial.println("false");
    }
  }
  if (isStart <= 0) {
    if(running){
      totalhalt();    // stop!
    }
    int findsomebody = digitalRead(Sharppin);
    if(DEBUG){
      Serial.print("findsomebody:");  
      Serial.println(findsomebody);  
    }   
    if(findsomebody > 0) {
      isStart = maxStart;
    }
    delay(4000);
    return;
  }
  isStart--;
  delay(100);
  if(DEBUG){
    Serial.print("isStart: ");
    Serial.println(isStart); 
  }
  currDist = MeasuringDistance(); //读取前端距离
  if(DEBUG){
    Serial.print("Current Distance: ");
    Serial.println(currDist); 
  } 
  if(currDist > 30) {
    nodanger();
  }
  else if(currDist < 15){
    backup();
    randTrun();
  }
  else {
    //whichway();
    randTrun();
  }
}
//测量距离 单位厘米
  long MeasuringDistance() {
  long duration;
  //pinMode(TrigPin, OUTPUT);
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(TrigPin, LOW);
  //pinMode(EchoPin, INPUT);
  duration = pulseIn(EchoPin, HIGH);
  return duration / 29 / 2;
}
// 前进，此段中，两个255均为最大转速，为了使小车无障碍前进时能保持直线，建议对这两个数字多次调试更改
void nodanger() {
  running = true;
  digitalWrite(leftmotorpin1, HIGH);
  analogWrite(leftmotorpin2, 255);
  digitalWrite(rightmotorpin1, HIGH);
  analogWrite(rightmotorpin2, 255);
  return;
} 
//后退
void backup() {
  running = true;
  digitalWrite(leftmotorpin1, LOW);
  analogWrite(leftmotorpin2, 100);
  digitalWrite(rightmotorpin1, LOW);
  analogWrite(rightmotorpin2, 100);
  delay(1000);
}
//选择路线
void whichway() {
  running = true;
  totalhalt();    // first stop!
  headservo.write(20);
  delay(1000);
  int lDist = MeasuringDistance();   // check left distance
  totalhalt();      // 恢复探测头
  headservo.write(120);  // turn the servo right
  delay(1000);
  int rDist = MeasuringDistance();   // check right distance
  totalhalt();      // 恢复探测头
  if(lDist < rDist) {
    body_lturn();
  }
  else{
    body_rturn();
  }
  return;
}
//重新机械调整到初始状态
void totalhalt() {
  digitalWrite(leftmotorpin1, HIGH);
  analogWrite(leftmotorpin2, 20);
  digitalWrite(rightmotorpin1, HIGH);
  analogWrite(rightmotorpin2, 20);
  headservo.write(70);  //  set servo to face forward
  running = false;
  return;
  delay(1000);
} 
//左转
void body_lturn() {
  running = true;
  digitalWrite(leftmotorpin1, LOW);
  analogWrite(leftmotorpin2, 100);
  digitalWrite(rightmotorpin1, HIGH);
  analogWrite(rightmotorpin2, 100);
  delay(1500);
  totalhalt();
} 
//右转
void body_rturn() {
  running = true;
  digitalWrite(leftmotorpin1, HIGH);
  analogWrite(leftmotorpin2, 100);
  digitalWrite(rightmotorpin1, LOW);
  analogWrite(rightmotorpin2, 100);
  delay(1500);
  totalhalt();
} 
void randTrun(){
  long randNumber;
  randomSeed(analogRead(0));
  randNumber = random(0, 10);
  if(randNumber > 5) {
    body_rturn();
  }
  else
  {
    body_lturn();
  }
}

谢谢分享！
其他同学可以测试此程序，提高完善该程序。