(4WD+2路红外线)arduino UNO四驱arduino超声波+红外线避障小车

有兴趣可以点击**我的店铺**看看小车硬件。提供安装说明。此图片仅供参考，图中是2驱超声波避障+循迹双模式小车。程序与本文有相同之处。

。。。。。。。。。。。。。。。。。。。。

四驱arduino超声波+红外线避障小车。以下是我优化后的代码

#include <Servo.h>

int pinLB = 5; //左退
int pinLF = 4; //左前
int pinRB = 3; //右退
int pinRF = 2; //右前
int inputPin = A0;//超声波Echo引脚
int outputPin = A1; //超声波Trig引脚
int servoPin = 13; //舵机信息号引脚
const int SensorLeft = 7; //左红外传感器管引脚
const int SensorRight = 8; //右红外传感器管引脚

int MotorRPWM=6;
int MotorLPWM=11;

int SL; //左
int SR; //右

int Fspeedd = 0;
int Rspeedd = 0;
int Lspeedd = 0;
int directionn = 0;
Servo myservo;//定义舵机
int delay_time = 250;

int Fgo = 1;
int Rgo = 2;
int Lgo = 3;
int Bgo = 4;

int F = 90;
int L1 = 170;
int L2 = 130;
int R1 = 10;
int R2 = 50;

int F_space = 0;
int Stime = 0;//转向用时

int L_space = 0;
int L1_space = 0;
int L2_space = 0;

int R_space = 0;
int R1_space = 0;
int R2_space = 0;

void setup()

{

Serial.begin(9600);
pinMode(pinLB, OUTPUT);
pinMode(pinLF, OUTPUT);
pinMode(pinRB, OUTPUT);
pinMode(pinRF, OUTPUT);
pinMode(inputPin, INPUT);
pinMode(outputPin, OUTPUT);
myservo.attach(servoPin);//定义舵机信息号引脚
pinMode(SensorLeft, INPUT); //定义左红外传感器
pinMode(SensorRight, INPUT); //定义右红外传感器
pinMode(MotorLPWM, OUTPUT); //PWM
pinMode(MotorRPWM, OUTPUT);

}

void advance(int a) //前进
{
digitalWrite(pinRB, LOW);
digitalWrite(pinRF, HIGH);
analogWrite(MotorRPWM,100);
digitalWrite(pinLB, LOW);
digitalWrite(pinLF, HIGH);
analogWrite(MotorLPWM,100);
delay(a * 100);
}
void right(int b)//单轮右转
{
digitalWrite(pinRB, HIGH);
digitalWrite(pinRF, HIGH);
digitalWrite(pinLB, LOW);
digitalWrite(pinLF, HIGH);
analogWrite(MotorLPWM,150);

delay(b * 100);
}

void left(int c)//单轮左转
{
digitalWrite(pinRB, LOW);
digitalWrite(pinRF, HIGH);
digitalWrite(pinLB, HIGH);
digitalWrite(pinLF, HIGH);
analogWrite(MotorRPWM,150);
delay(c * 100);
}

void turnR(int d)//双轮逆向转动，转右
{
digitalWrite(pinRB, HIGH);
digitalWrite(pinRF, LOW);
analogWrite(MotorRPWM,150);

digitalWrite(pinLB, LOW);
digitalWrite(pinLF, HIGH);
analogWrite(MotorLPWM,150);

delay(d * 100);
}

void turnL(int e)//双轮逆向转动，转左
{
digitalWrite(pinRB, LOW);
digitalWrite(pinRF, HIGH);
analogWrite(MotorRPWM,150);

digitalWrite(pinLB, HIGH);
digitalWrite(pinLF, LOW);
analogWrite(MotorLPWM,150);

delay(e * 100);
}

void stopp(int f)//停止
{
digitalWrite(pinRB, HIGH);
digitalWrite(pinRF, HIGH);
digitalWrite(pinLB, HIGH);
digitalWrite(pinLF, HIGH);
delay(f * 100);
}

void back(int g)//后退
{
digitalWrite(pinRB, HIGH);
digitalWrite(pinRF, LOW);
analogWrite(MotorRPWM,100);

digitalWrite(pinLB, HIGH);
digitalWrite(pinLF, LOW);
analogWrite(MotorLPWM,100);

delay(g * 100);
}

int L_max_space(int jiaodu1, int jiaodu2)//得到左边两个角度最大距离值
{
L1_space = ask_distance(jiaodu1);
L2_space = ask_distance(jiaodu2);
if (L1_space > L2_space)
{
Stime = 4;
return ( L1_space);
} else
{
Stime = 1;
return ( L2_space);
}
}

int R_max_space(int jiaodu1, int jiaodu2)//得到右边两个角度最大距离值
{
R1_space = ask_distance(jiaodu1);
R2_space = ask_distance(jiaodu2);
if (R1_space > R2_space)
{
Stime = 4;
return ( R1_space);
} else
{
Stime = 1;
return ( R2_space);
}
}

void detection()
{
int delay_time = 250;
F_space = ask_distance(F);
if (F_space < 10)//前方小于10公分，后退
{
stopp(1);
back(2);
}
if (F_space < 25)//前方小于25公分
{
stopp(1);
L_space = L_max_space(L1, L2);//得到左边距离值
R_space = R_max_space(R1, R2);//得到右边距离值

if (L_space > R_space)//如果左>右，左转
{
  directionn = Lgo;
}
if (L_space <= R_space)//如果左<=右，右转
{
  directionn = Rgo;
}
if (L_space < 10 && R_space < 10)//均小于10公分，后退
{
  directionn = Bgo;
}

}
else
{
directionn = Fgo;//否则前进
}
}

int ask_distance(int angle)
{

myservo.write(angle);
delay(delay_time);
digitalWrite(outputPin, LOW); // 超声波发射低电平2μs
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // 超声波发射高电平10μs
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // 维持超声波发射低电平
float distance = pulseIn(inputPin, HIGH) / 58.00; //读取相差时间,将时间转化为距离（单位：厘米）
//Serial.print(“distance:”);
//Serial.println(distance);
return (distance);
}

void loop()
{

myservo.write(90);//摆正
detection();//检测距离，判断需要往哪个方向走。
SL = digitalRead(SensorLeft);//红外检测左边障碍物
SR = digitalRead(SensorRight);//红外检测右边障碍物

if (directionn == 4)//后退
{
back(5);
turnL(2);//稍微左转，防止卡死.这里应该设置一下随机，微左转或微右转
//Serial.println(" Reverse ");
}

if (directionn == 2)//右转
{
back(1);
turnR(Stime);//双轮逆向转动，转右，给于转向时间
//Serial.print(" Right ");
}

if (directionn == 3)//左转
{
back(1);
turnL(Stime);//双轮逆向转动，转左，给于转向时间
//Serial.print(" Left ");
}

if (directionn == 1)//前进
{
advance(1);
//Serial.print(" Advance “);
//Serial.println(” ");

if (SL == HIGH & SR == LOW)  // 如果右边红外线感测到物体，向左（低电平LOW表示有障外物）
{
  left(4);//单轮转左
}
else if (SR == HIGH & SL == LOW) // 如果左边红外线感应到物体，向右
{
  right(4);//单轮转右
}
else if (SR == LOW & SL == LOW) //  如果两个红外线均感应到物体，后退
{
  back(3);
}

}
}