Arduino超声波避障小车

#define DEBUG 0    // set to 1 to print to serial monitor, 0 to disable
#include <Servo.h>

Servo headservo;  // 头部舵机对象

// Constants
const int EchoPin = 2; //超声波信号输入
const int TrigPin = 3; //超声波控制信号输出

const int leftmotorpin1 = 4; // 直流电机信号输出
const int leftmotorpin2 = 5;
const int rightmotorpin1 = 6;
const int rightmotorpin2 = 7;

const int HeadServopin = 9; // 舵机信号输出 只有9或10接口可利用
const int Sharppin = 11; // 红外输入 

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(Sharppin, INPUT);

  //信号输出接口
  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;
}

// 前进
void nodanger() {
  running = true;
  digitalWrite(leftmotorpin1, LOW);
  digitalWrite(leftmotorpin2, HIGH);
  digitalWrite(rightmotorpin1, LOW);
  digitalWrite(rightmotorpin2, HIGH);
  return;
}  

//后退
void backup() {
  running = true;
  digitalWrite(leftmotorpin1, HIGH);
  digitalWrite(leftmotorpin2, LOW);
  digitalWrite(rightmotorpin1, HIGH);
  digitalWrite(rightmotorpin2, LOW);
  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);
  digitalWrite(leftmotorpin2, HIGH);
  digitalWrite(rightmotorpin1, HIGH);
  digitalWrite(rightmotorpin2, HIGH);
  headservo.write(70);  //  set servo to face forward
  running = false; 
  return;
  delay(1000);
}  

//左转
void body_lturn() {
  running = true;
  digitalWrite(leftmotorpin1, LOW);
  digitalWrite(leftmotorpin2, HIGH);
  digitalWrite(rightmotorpin1, HIGH);
  digitalWrite(rightmotorpin2, LOW);
  delay(1500);
  totalhalt();
}  

//右转
void body_rturn() {
  running = true;
  digitalWrite(leftmotorpin1, HIGH);
  digitalWrite(leftmotorpin2, LOW);
  digitalWrite(rightmotorpin1, LOW);
  digitalWrite(rightmotorpin2, HIGH);
  delay(1500);
  totalhalt();
}  

void randTrun(){
  long randNumber;
  randomSeed(analogRead(0));
  randNumber = random(0, 10);
  if(randNumber > 5) {
    body_rturn();
  }
  else
  {
    body_lturn();
  }
}
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.
111.
112.
113.
114.
115.
116.
117.
118.
119.
120.
121.
122.
123.
124.
125.
126.
127.
128.
129.
130.
131.
132.
133.
134.
135.
136.
137.
138.
139.
140.
141.
142.
143.
144.
145.
146.
147.
148.
149.
150.
151.
152.
153.
154.
155.
156.
157.
158.
159.
160.
161.
162.
163.
164.
165.
166.
167.
168.
169.
170.
171.
172.
173.
174.
175.
176.
177.
178.
179.
180.
181.
182.
183.
184.
185.
186.
187.
188.
189.
190.
191.
192.
193.
194.
195.
196.
197.
198.
199.
200.
201.
202.
203.
204.
205.
206.
207.
208.