Arduino智能小车的走路程序

quasiceo 2015-08-09

展开全文

Arduino智能小车的走路程序

发表于1年前(2014-05-24 12:28) 阅读（782） | 评论（1） 2人收藏此文章, 我要收藏

摘要本文提供了Arduino小车的沿墙走程序代码和视频

Arduino 程序

功能就是沿着一面墙走。非常简单，但是手头上的这个小车非常的差，做工不好不说，配的两边的电机参数差别非常大，同样的控制PWM，两侧的速度能相差一倍，更别提行走时晃晃悠悠的差点就掉下来的轮子了。另外，只配备了一个超声波测距模块，，不能同时检测多个方向上的障碍物，是这个软件难编写的一个原因。

视频地址：

http://v.youku.com/v_show/id_XNzAyOTg0Nzc2.html

代码：

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
int outputPin = 5;
int inputPin = 4;
int cmdPin = 3;
int pinI1=8;//定义I1接口
int pinI2=9;//定义I2接口
int speedpin1=11;//定义EA(PWM调速)接口
int pinI3=6;//定义I3接口
int pinI4=7;//定义I4接口
int speedpin2=10;//定义EB(PWM调速)接口
int average_speed_l = 150;
int average_speed_r = 100;
int speed_limit_l = 80;
int speed_limit_r = 60;
int target_distance = 30;//in cm
int lost_distance = 80;
float detect_distance()
{
      digitalWrite(outputPin, LOW);   // 讓超聲波發射低電壓2μs
      delayMicroseconds(2);
      digitalWrite(outputPin, HIGH);  // 讓超聲波發射高電壓10μs，這裡至少是10μs
      delayMicroseconds(10);
      digitalWrite(outputPin, LOW);    // 維持超聲波發射低電壓
      float Fdistance = pulseIn(inputPin, HIGH);  // 讀差相差時間
      Fdistance= Fdistance/5.8/10;       // 將時間轉為距離距离（單位：公分）
      Serial.print("F distance:");      //輸出距離（單位：公分）
      Serial.println(Fdistance);         //顯示距離
      return Fdistance;
}
void rightSpeed(int speed_r)
{
  if(speed_r < speed_limit_r)
  {
    speed_r = 0;
  }
analogWrite(speedpin1,speed_r);//输入模拟值进行设定速度
digitalWrite(pinI1,HIGH);//使直流电机（左）顺时针转
digitalWrite(pinI2,LOW);
}
void leftSpeed(int speed_l)
{
  if(speed_l < speed_limit_l)
  {
    speed_l = 0;
  }
   analogWrite(speedpin2,speed_l);//输入模拟值进行设定速度
   digitalWrite(pinI3,LOW);//使直流电机（）顺时针转
   digitalWrite(pinI4,HIGH);
}
void stop_l()
{
   digitalWrite(speedpin2,LOW);//输入模拟值进行设定速度
   digitalWrite(pinI3,LOW);//使直流电机（）顺时针转
   digitalWrite(pinI4,LOW);
}
void stop_r()
{
digitalWrite(speedpin1,LOW);//输入模拟值进行设定速度
digitalWrite(pinI1,LOW);//使直流电机（左）顺时针转
digitalWrite(pinI2,LOW);
}
void turn_l()
{
  leftSpeed(100);
  rightSpeed(150);
}
void turn_r()
{
  leftSpeed(200);
  rightSpeed(80);
}
void noturn()
{
  leftSpeed(100);
  rightSpeed(80);
}
void rotate()
{
  leftSpeed(200);
  rightSpeed(0);
}
void setup()
{
  Serial.begin(9600);
  pinMode(outputPin, OUTPUT);
  pinMode(inputPin, INPUT);
  pinMode(pinI1,OUTPUT);
  pinMode(pinI2,OUTPUT);
  pinMode(speedpin1,OUTPUT);
  pinMode(pinI3,OUTPUT);
  pinMode(pinI4,OUTPUT);
  pinMode(speedpin2,OUTPUT);
  pinMode(cmdPin, INPUT);
}
void loop()
{
  if(digitalRead(cmdPin) == LOW){

    float dis = detect_distance();
    if(dis > lost_distance){
      while(true)
      {
        rotate();
        delay(100);
        stop_l();
        stop_r();
        delay(100);
        dis = detect_distance();
        if(dis < lost_distance) break;
      }
    }else{
      if(dis > target_distance){
        if(dis - target_distance > 2){ //if the difference is small enough, it can be neglected.
          turn_r();
        }else{
          noturn();
        }
      }
      if(dis < target_distance){
        if(target_distance - dis > 2){
          turn_l();
        }else{
          noturn();
        }
      }
    }
   }else{
      stop_l();
      stop_r();
   }
  delay(100);
}