有兴趣可以点击**我的店铺**看看小车硬件。提供安装说明。此图片仅供参考,图中是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);
}
}
}