超声波避障小车Arduino源码 -

#include <AFMotor.h>
//#include <MsTimer2.h>
#define PCA9685_adrr 0x80// 1+A5+A4+A3+A2+A1+A0+w/r
//片选地址，将焊接点置1可改变地址，
// 当IIC总呱嫌多片PCA9685或相同地址时才需焊接
#define PCA9685_SUBADR1 0x2
#define PCA9685_SUBADR2 0x3
#define PCA9685_SUBADR3 0x4
#define PCA9685_MODE1 0x0//PCA9685模式复位设置
#define PCA9685_PRESCALE 0xFE//控制周期的寄存器
#define LED0_ON_L 0x6
#define LED0_ON_H 0x7
#define LED0_OFF_L 0x8
#define LED0_OFF_H 0x9
#define ALLLED_ON_L 0xFA
#define ALLLED_ON_H 0xFB
#define ALLLED_OFF_L 0xFC
#define ALLLED_OFF_H 0xFD
#define SERVOMIN 115 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 590 // this is the 'maximum' pulse length count (out of 4096)
//#define SERVO000 130 //0度对应4096的脉宽计数值
#define SERVO000 145 //0度对应4096的脉宽计数值
#define SERVO180 520 //180度对应4096的脉宽计算值，四个值可根据不同舵机修改
AF_DCMotor LF_motor(1);//定义四个车轮电机对应的内存单元
AF_DCMotor RF_motor(3);
AF_DCMotor LB_motor(2);
AF_DCMotor RB_motor(4);
// 设定SR04连接的Arduino引脚
const int TrigPin = A0;//超声波测距使能控制信号
const int EchoPin = A1;//超声波测距模块输出，通过测得高电平持续时间获得障碍物距离
//unsigned long distance;
const int IIC_SDA = A5;//串行数据线信号
const int IIC_SCL = A4;//串行时钟线信号
const int EchoPin_L = A2;//左舵机超声波探测器接收信号
const int EchoPin_R = A3;//右舵机超声波探测器接收信号
boolean stage1_ctrl=false;//前端超声波云台控制信号，true表示当前从60度转到0度；
boolean stage23_ctrl=false;//左右端超声波云台控制信号，true表示当前从60度转到0度；
boolean direction_ctrl_r=true;
boolean direction_ctrl_l=true;
int l=0;
int m=0;
int stage1_dis;
int stage2_dis;
int stage3_dis;
void setup()
{ // 初始化串口通信及连接SR04的引脚
//Serial.begin(9600);
pinMode(TrigPin, OUTPUT);
// 要检测引脚上输入的脉冲宽度，需要先设置为输入状态
pinMode(EchoPin, INPUT);
//Serial.println("开始");
LF_motor.setSpeed(150);
RF_motor.setSpeed(150);
LB_motor.setSpeed(150);
RB_motor.setSpeed(150);
LF_motor.run(FORWARD);
RF_motor.run(FORWARD);
LB_motor.run(FORWARD);
RB_motor.run(FORWARD);
pinMode(IIC_SDA,OUTPUT);
pinMode(IIC_SCL,OUTPUT);
//Serial.println("SDA,SCL引脚初始化");
IIC_init();
// Serial.println("IIC初始化");
reset();
// Serial.println("初始化结束");
setPWMFreq(50);
// Serial.println("初始化结束");
//MsTimer2::set(500, stage1); // 500ms period定时器2初始化；
// Serial.println("初始化结束");
// MsTimer2::start();
//Serial.println("初始化结束");
}
void loop()
{// Serial.println("主程序");
if(stage1_ctrl==true)
{
stage1_ctrl=false;
setPWM(0, 0, SERVO000);//第0路舵机从60度转到0角度 0.12-0.13秒/60度
}
else
{
stage1_ctrl=true;
setPWM(0, 0,224); //第0路舵机从0度转到60角度
}
for(l=0;l<=100;l++)
{
// 产生一个1。、0us的高脉冲去触发TrigPin,即开启超声波测距
digitalWrite(TrigPin, LOW);
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
// 检测脉冲宽度，并计算出距离
delayMicroseconds(2);
int stage1_dis=pulseIn(EchoPin, HIGH,2000);
//Serial.println(stage1_dis);
//distance = pulseIn(EchoPin, HIGH) / 58.00;
if(stage1_dis>0)//stage1_dis=0表示超过超声波等待回收信号范围，即距离超过（1300/58）cm；
{
if(stage23_ctrl==true)
{
stage23_ctrl=false;
setPWM(1, 0, SERVO000);//第1路舵机从60度转到0角度
setPWM(2, 0,224);//第2路舵机从0度转到60角度
}
else
{
stage23_ctrl=true;
setPWM(1, 0,224); //第1路舵机从0度转到60角度
setPWM(2, 0, SERVO000); //第1路舵机从60度转到0角度
}
// Serial.println("外循环");
for(m=0;m<=40;m++)
{
//Serial.println("前端超声波探测循环");
//delay(1);
digitalWrite(TrigPin, LOW); // 产生一个10us的高脉冲去触发TrigPin,即开启超声波测距
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
delayMicroseconds(2);
stage2_dis=pulseIn(EchoPin_L, HIGH,1500);
delay(1);//如果不延时处理，stage3_dis测得距离会有问题；
digitalWrite(TrigPin, LOW);
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
delayMicroseconds(2);
stage3_dis=pulseIn(EchoPin_R, HIGH,1500);
//Serial.println(stage1_dis);
//Serial.println(stage2_dis);
// Serial.println(stage3_dis);
//delay(500);
if(stage2_dis>0)
{
direction_ctrl_l=false;
}
else
{
direction_ctrl_l=true;
}
if(stage3_dis>0)
{
direction_ctrl_r=false;
}
else
{
direction_ctrl_r=true;
}
}
// Serial.println(direction_ctrl_l);
//Serial.println(direction_ctrl_r);
if(direction_ctrl_l==true)
{
// Serial.println("左转");
LF_motor.run(BACKWARD);
LB_motor.run(BACKWARD);
// RF_motor.run(FORWARD);
// RB_motor.run(FORWARD);
delay(1000);
// LF_motor.run(FORWARD);
// LB_motor.run(FORWARD);
}
else if(direction_ctrl_r==true)
{
// Serial.println("右转");
RF_motor.run(BACKWARD); //如果之前左转，发现左端忽然有障碍物出现，则机器人实际变成后退动作；
RB_motor.run(BACKWARD);
//LF_motor.run(FORWARD);
//LB_motor.run(FORWARD);
delay(1000);
// RF_motor.run(FORWARD);
// RB_motor.run(FORWARD);
}
else
{
// Serial.println("后退");
RF_motor.run(BACKWARD);
RB_motor.run(BACKWARD);
LF_motor.run(BACKWARD);
LB_motor.run(BACKWARD);
delay(1000);
// RF_motor.run(FORWARD);
// RB_motor.run(FORWARD);
// LF_motor.run(FORWARD);
// LB_motor.run(FORWARD);
}
break;//注意内层的break只能跳出内层的for循环，两个，三个break，也都只能跳出本层循环，及第二个Break根本不会执行；
//digitalWrite(LF, HIGH);
//digitalWrite(LB, HIGH);
}
else
{
RF_motor.run(FORWARD);
RB_motor.run(FORWARD);
LF_motor.run(FORWARD);
LB_motor.run(FORWARD);
}
}
//Serial.print(distance);
//Serial.print("cm");
//Serial.println();
//delay(500);
}
//IIC初始化
void IIC_init()
{
digitalWrite(IIC_SDA,HIGH); //IIC_SDA IIC_SCL使用前被拉高
delayMicroseconds(4);
digitalWrite(IIC_SCL,HIGH);
delayMicroseconds(4);
}
void start()//基于IIC总线的物理结构，总线上的START和IIC_stop信号必定是唯一的。
//另外，IIC总线标准规定IIC_SDA线的数据转换必须在IIC_SCL线的低电平期，
//在IIC_SCL线的高电平期，IIC_SDA线的上数据是稳定的。所以，start()，IIC_stop()的
//特殊情况可以作为指示的起止信号。
{
// Serial.println("start开始");
digitalWrite(IIC_SDA,HIGH);
delayMicroseconds(4);
digitalWrite(IIC_SCL,HIGH); //IIC_SCL高 IIC_SDA拉低表示可以IIC启动
delayMicroseconds(4);
digitalWrite(IIC_SDA,LOW);
delayMicroseconds(4);
digitalWrite(IIC_SCL,LOW);
delayMicroseconds(4);
// Serial.println("start结束");
}
//IIC停止
void IIC_stop()
{
digitalWrite(IIC_SDA,LOW);
delayMicroseconds(4);
digitalWrite(IIC_SCL,HIGH);
delayMicroseconds(4);
digitalWrite(IIC_SDA,HIGH);
delayMicroseconds(4);
}
//IIC应答
void ACK()//IIC_SDA应答信号为0表示正常收发数据，为1表示停止，
{
unsigned char i;
digitalWrite(IIC_SCL,HIGH);
delayMicroseconds(2);
pinMode(IIC_SDA,INPUT);
while((digitalRead(IIC_SDA))&&(i<=255))
{
i++;
}
pinMode(IIC_SDA,OUTPUT);
digitalWrite(IIC_SCL,LOW);
delayMicroseconds(4);
}
//写字节
void write_byte(unsigned char byte)
{
//Serial.println("地址写入SDL开始");调试用
unsigned char i,temp;
temp=byte;
for(i=7;i<=7;i--)
{//unsigned char x;//调试用
//x=0;//调试用
//const int test = 2;//调试用
//pinMode(test,INPUT);//调试用
digitalWrite(IIC_SCL,LOW);
delayMicroseconds(4);
//Serial.println(temp);//调试用
//Serial.println(bitRead(temp,i));//调试用
digitalWrite(IIC_SDA,bitRead(temp,i));
//Serial.println(digitalRead(test));
// x=(digitalRead(test))|(x<<1);//调试用
//Serial.println(x);//调试用
//Serial.println(i);//调试用
delayMicroseconds(4);
digitalWrite(IIC_SCL,HIGH);
delayMicroseconds(4);
}
digitalWrite(IIC_SCL,LOW);
delayMicroseconds(4);
digitalWrite(IIC_SDA,HIGH);
delayMicroseconds(4);
}
unsigned char read_byte()
{
unsigned char i,j,k;
digitalWrite(IIC_SCL,LOW);
delayMicroseconds(4);
digitalWrite(IIC_SDA,HIGH);
delayMicroseconds(4);
for(i=0;i<8;i++)
{
delayMicroseconds(4);
digitalWrite(IIC_SCL,HIGH);
delayMicroseconds(4);
pinMode(IIC_SDA,INPUT);
if(digitalRead(IIC_SDA))
{
j=1;
}
else
{
j=0;
}
pinMode(IIC_SDA,OUTPUT);
k=(k<<1)|j;
digitalWrite(IIC_SCL,LOW);
}
delayMicroseconds(4);
return k;
}
void PCA9685_write(unsigned char address,unsigned char date)
{
//Serial.println("PCA9685_write开始");
start();
write_byte(PCA9685_adrr); //PCA9685
ACK();
write_byte(address); //PCA9685对应内部哪个地址
ACK();
write_byte(date); //所写入的数据
//Serial.println("所写入的数据:");
//Serial.println(date);
ACK();
IIC_stop();
//Serial.println("PCA9685_write结束");
}
//从PCA9685读数据有返回值
unsigned char PCA9685_read(unsigned char address)
{
unsigned char date;
start();
write_byte(PCA9685_adrr); //PCA9685
ACK();
write_byte(address);//PCA9685对应内部哪个地址
ACK();
start();
write_byte(PCA9685_adrr|0x01); //表示此时状态为从PCA9685读取数据
ACK();
date=read_byte();
// Serial.println("date");
//Serial.println(date);
return date;
}
//PCA9685复位
void reset(void)
{//Serial.println("RESET开始");
PCA9685_write(PCA9685_MODE1,0x0);
//Serial.println("RESET结束");
}
//PCA9685修改频率
/*---------------------------------------------------------------
PCA9685修改角度函数
num:舵机PWM输出引脚0~15,on:PWM上升计数值0~4096,off:PWM下降计数值0~4096
一个PWM周期分成4096份,由0开始+1计数,计到on时跳变为高电平,继续计数到off时
跳变为低电平,直到计满4096重新开始。所以当on不等于0时可作延时,当on等于0时,
off/4096的值就是PWM的占空比。
----------------------------------------------------------------*/
void setPWMFreq(float freq)
{ //Serial.print("设置PWM周期：");
unsigned int prescale,oldmode,newmode;
float prescaleval;
freq *= 0.915;
prescaleval = 25000000;
prescaleval /= 4096;
prescaleval /= freq;
prescaleval -= 1;
prescale = floor(prescaleval + 0.5);
oldmode = PCA9685_read(PCA9685_MODE1);
newmode = (oldmode&0x7F) | 0x10; // sleep
//Serial.println("新模式：");
PCA9685_write(PCA9685_MODE1,newmode); // go to sleep
// Serial.println("新模式：");
// Serial.println(newmode);
PCA9685_write(PCA9685_PRESCALE,prescale); // set the prescaler
// Serial.println("周期频率设置值：");
// Serial.println(prescale);
PCA9685_write(PCA9685_MODE1,oldmode);
// Serial.println("旧模式：");
// Serial.println(oldmode);
delay(2);
PCA9685_write(PCA9685_MODE1,oldmode | 0xa1);
}
void setPWM(unsigned int num, unsigned int on, unsigned int off)
{
//Serial.print("当前通道：");
//Serial.print(num);
PCA9685_write(LED0_ON_L+4*num,on);
PCA9685_write(LED0_ON_H+4*num,on>>8);
PCA9685_write(LED0_OFF_L+4*num,off);
PCA9685_write(LED0_OFF_H+4*num,off>>8);
}
/*void stage1()
{
Serial.print("timer");
if(stage1_ctrl==true)
{
stage1_ctrl=false;
setPWM(0, 0, SERVO000);//第0路舵机从60度转到0角度
}
else
{
stage1_ctrl=true;
setPWM(0, 0,224); //第0路舵机从0度转到60角度
}
//setPWM(0, 0, SERVO180);//第0路舵机转到0角度
//setPWM(1, 0, SERVO000);//第1路舵机转到0角度
}
*/

复制代码