|
|
本帖最后由 周成瑞 于 2019-12-5 15:01 编辑
#include <Servo.h> //调用舵机函数库
#define enable_L 13 //左轮
#define INA1 3
#define INA2 5
#define enable_R A4 //右轮
#define INTB1 6
#define INTB2 9
int myangle;//定义角度变量
int pulsewidth;//定义脉宽变量
int val;
int Front_Distance = 0;//
int Left_Distance = 0;
int Right_Distance = 0;
const int Trig = 7; //超声波
const int Echo = 8;
int myservo=10;//设置舵机驱动脚到数字口2
void setup()
{
Serial.begin(9600);
/*引脚初始化*/
pinMode(enable_L,OUTPUT);
pinMode(INA1,OUTPUT);
pinMode(INA2,OUTPUT);
pinMode(enable_R,OUTPUT);
pinMode(INTB1,OUTPUT);
pinMode(INTB2,OUTPUT);
pinMode(myservo,OUTPUT);
/*电机驱动使能*/
digitalWrite(enable_L,HIGH); // 左电机使能
digitalWrite(enable_R,HIGH); // 右电机使能
pinMode(Trig, OUTPUT); //超声波
pinMode(Echo, INPUT);
}
/*===========================小车基本动作===================
小车运动函数
后退Back
前进Forward
左转Left
右转Right
刹车Brake
*/
void Back(int time) //小车后转
{
analogWrite(INA1,95);
analogWrite(INA2,LOW); //左PWM 调速
analogWrite(INTB1,100);
analogWrite(INTB2,LOW); //右PWM 调速
delay(time * 100); //执行时间，可以调整
}
void Right(int time) //小车左转(右轮不动，左轮前进)
{
analogWrite(INA1,LOW);
analogWrite(INA2,LOW); //左PWM 调速
analogWrite(INTB1,LOW);
analogWrite(INTB2,100); //右PWM 调速
delay(time * 100); //执行时间，可以调整
}
void Forward() //小车前进
{
analogWrite(INA1,LOW);
analogWrite(INA2,100); //左PWM 调速
analogWrite(INTB1,LOW);
analogWrite(INTB2,100); //右PWM 调速
}
void Left(int time) //小车右转(左轮不动，右轮前进)
{
analogWrite(INA1,LOW);
analogWrite(INA2,100); //左PWM 调速
analogWrite(INTB1,LOW);
analogWrite(INTB2,LOW); //右PWM 调速
delay(time * 100); //执行时间，可以调整
}
void spin_left(int time) //左转(左轮后退，右轮前进)
{
analogWrite(INA1,LOW);
analogWrite(INA2,100); //左PWM 调速
analogWrite(INTB1,100);
analogWrite(INTB2,HIGH); //右PWM 调速
delay(time * 100); //执行时间，可以调整
}
void Brake(int time) //刹车
{
analogWrite(INA1,LOW);
analogWrite(INA2,LOW); //左PWM 调速
analogWrite(INTB1,LOW);
analogWrite(INTB2,LOW); //右PWM 调速
delay(time * 100); //执行时间，可以调整
}
//==================超声波===============
float Distance_test() // 量出前方距离
{
digitalWrite(Trig, LOW); // 给触发脚低电平2μs
delayMicroseconds(2);
digitalWrite(Trig, HIGH); // 给触发脚高电平10μs，这里至少是10μs
delayMicroseconds(10);
digitalWrite(Trig, LOW); // 持续给触发脚低电
float Fdistance = pulseIn(Echo, HIGH); // 读取高电平时间(单位：微秒)
Fdistance= Fdistance/58; //为什么除以58等于厘米， Y米=（X秒*344）/2
return Fdistance;
}
void servopulse(int myservo,int myangle)/*定义一个脉冲函数，用来模拟方式产生PWM值舵机的范围是0.5MS到2.5MS 1.5MS 占空比是居中周期是20MS*/
{
pulsewidth=(myangle*11)+500;//将角度转化为500-2480 的脉宽值 这里的myangle就是0-180度 所以180*11+50=2480 11是为了换成90度的时候基本就是1.5MS
digitalWrite(myservo,HIGH);//将舵机接口电平置高 90*11+50=1490uS 就是1.5ms
delayMicroseconds(pulsewidth);//延时脉宽值的微秒数 这里调用的是微秒延时函数
digitalWrite(myservo,LOW);//将舵机接口电平置低
// delay(20-pulsewidth/1000);//延时周期内剩余时间 这里调用的是ms延时函数
delay(20-(pulsewidth*0.001));//延时周期内剩余时间 这里调用的是ms延时函数
}
void front_detection()
{
//此处循环次数减少，为了增加小车遇到障碍物的反应速度
for(int i=0;i<=5;i++) //产生PWM个数，等效延时以保证能转到响应角度
{
servopulse(myservo,90);//模拟产生PWM
}
Front_Distance = Distance_test();
}
void left_detection()
{
for(int i=0;i<=15;i++) //产生PWM个数，等效延时以保证能转到响应角度
{
servopulse(myservo,175);//模拟产生PWM
}
Left_Distance = Distance_test();
}
void right_detection()
{
for(int i=0;i<=15;i++) //产生PWM个数，等效延时以保证能转到响应角度
{
servopulse(myservo,5);//模拟产生PWM
}
Right_Distance = Distance_test();
}
void loop()
{
while(1)
{
front_detection();//测量前方距离
if(Front_Distance < 30)//当遇到障碍物时
{
Back(2);//后退减速
Brake(2);//停下来做测距
left_detection();//测量左边距障碍物距离
right_detection();//测量右边距障碍物距离
if((Left_Distance < 30 ) &&( Right_Distance < 30 ))//当左右两侧均有障碍物靠得比较近
spin_left(0.7);//旋转掉头
else if(Left_Distance > Right_Distance)//左边比右边空旷
{
Left(3);//左转
Brake(1);//刹车，稳定方向
}
else//右边比左边空旷
{
Right(3);//右转
Brake(1);//刹车，稳定方向
}
}
else
{
Forward(); //无障碍物，直行
}
}
}
|
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
