Arduino智能小车的走路程序

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

视频地址：

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

代码：

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