標(biāo)題:
超聲波避障小車Arduino源碼
[打印本頁]
作者:
Riton
時間:
2018-8-25 12:09
標(biāo)題:
超聲波避障小車Arduino源碼
硬件電路配置有Arduino、L293D驅(qū)動模塊、PCA9685驅(qū)動模塊各一個,四個直流電機(jī)(由L293D驅(qū)動模塊驅(qū)動),3個舵機(jī)(PCA9685驅(qū)動模塊驅(qū)動),三個超聲波探測器。代碼有很多的多余注釋,還有想要用定時器2的草稿,大家看著做個參考。
//四輪獨立驅(qū)動小車,注意程序已經(jīng)舵機(jī)角度改為不是0-60之間變化,范圍有所縮小。
#include <AFMotor.h>
//#include <MsTimer2.h>
#define PCA9685_adrr 0x80// 1+A5+A4+A3+A2+A1+A0+w/r
//片選地址,將焊接點置1可改變地址,
// 當(dāng)IIC總 呱嫌 多片PCA9685或相同地址時才需焊接
#define PCA9685_SUBADR1 0x2
#define PCA9685_SUBADR2 0x3
#define PCA9685_SUBADR3 0x4
#define PCA9685_MODE1 0x0//PCA9685模式復(fù)位設(shè)置
#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度對應(yīng)4096的脈寬計數(shù)值
#define SERVO000 145 //0度對應(yīng)4096的脈寬計數(shù)值
#define SERVO180 520 //180度對應(yīng)4096的脈寬計算值,四個值可根據(jù)不同舵機(jī)修改
AF_DCMotor LF_motor(1);//定義四個車輪電機(jī)對應(yīng)的內(nèi)存單元
AF_DCMotor RF_motor(3);
AF_DCMotor LB_motor(2);
AF_DCMotor RB_motor(4);
// 設(shè)定SR04連接的Arduino引腳
const int TrigPin = A0;//超聲波測距使能控制信號
const int EchoPin = A1;//超聲波測距模塊輸出,通過測得高電平持續(xù)時間獲得障礙物距離
//unsigned long distance;
const int IIC_SDA = A5;//串行數(shù)據(jù)線信號
const int IIC_SCL = A4;//串行時鐘線信號
const int EchoPin_L = A2;//左舵機(jī)超聲波探測器接收信號
const int EchoPin_R = A3;//右舵機(jī)超聲波探測器接收信號
boolean stage1_ctrl=false;//前端超聲波云臺控制信號,true表示當(dāng)前從60度轉(zhuǎn)到0度;
boolean stage23_ctrl=false;//左右端超聲波云臺控制信號,true表示當(dāng)前從60度轉(zhuǎn)到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);
// 要檢測引腳上輸入的脈沖寬度,需要先設(shè)置為輸入狀態(tài)
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("初始化結(jié)束");
setPWMFreq(50);
// Serial.println("初始化結(jié)束");
//MsTimer2::set(500, stage1); // 500ms period定時器2初始化;
// Serial.println("初始化結(jié)束");
// MsTimer2::start();
//Serial.println("初始化結(jié)束");
}
void loop()
{// Serial.println("主程序");
if(stage1_ctrl==true)
{
stage1_ctrl=false;
setPWM(0, 0, SERVO000);//第0路舵機(jī)從60度轉(zhuǎn)到0角度 0.12-0.13秒/60度
}
else
{
stage1_ctrl=true;
setPWM(0, 0,224); //第0路舵機(jī)從0度轉(zhuǎn)到60角度
}
for(l=0;l<=100;l++)
{
// 產(chǎn)生一個1。、0us的高脈沖去觸發(fā)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路舵機(jī)從60度轉(zhuǎn)到0角度
setPWM(2, 0,224);//第2路舵機(jī)從0度轉(zhuǎn)到60角度
}
else
{
stage23_ctrl=true;
setPWM(1, 0,224); //第1路舵機(jī)從0度轉(zhuǎn)到60角度
setPWM(2, 0, SERVO000); //第1路舵機(jī)從60度轉(zhuǎn)到0角度
}
// Serial.println("外循環(huán)");
for(m=0;m<=40;m++)
{
//Serial.println("前端超聲波探測循環(huán)");
//delay(1);
digitalWrite(TrigPin, LOW); // 產(chǎn)生一個10us的高脈沖去觸發(fā)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("左轉(zhuǎn)");
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("右轉(zhuǎn)");
RF_motor.run(BACKWARD); //如果之前左轉(zhuǎn),發(fā)現(xiàn)左端忽然有障礙物出現(xiàn),則機(jī)器人實際變成后退動作;
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;//注意內(nèi)層的break只能跳出內(nèi)層的for循環(huán),兩個,三個break,也都只能跳出本層循環(huán),及第二個Break根本不會執(zhí)行;
//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總線的物理結(jié)構(gòu),總線上的START和IIC_stop信號必定是唯一的。
//另外,IIC總線標(biāo)準(zhǔn)規(guī)定IIC_SDA線的數(shù)據(jù)轉(zhuǎn)換必須在IIC_SCL線的低電平期,
//在IIC_SCL線的高電平期,IIC_SDA線的上數(shù)據(jù)是穩(wěn)定的。所以,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結(jié)束");
}
//IIC停止
void IIC_stop()
{
digitalWrite(IIC_SDA,LOW);
delayMicroseconds(4);
digitalWrite(IIC_SCL,HIGH);
delayMicroseconds(4);
digitalWrite(IIC_SDA,HIGH);
delayMicroseconds(4);
}
//IIC應(yīng)答
void ACK()//IIC_SDA應(yīng)答信號為0表示正常收發(fā)數(shù)據(jù),為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);
}
//寫字節(jié)
void write_byte(unsigned char byte)
{
//Serial.println("地址寫入SDL開始");調(diào)試用
unsigned char i,temp;
temp=byte;
for(i=7;i<=7;i--)
{//unsigned char x;//調(diào)試用
//x=0;//調(diào)試用
//const int test = 2;//調(diào)試用
//pinMode(test,INPUT);//調(diào)試用
digitalWrite(IIC_SCL,LOW);
delayMicroseconds(4);
//Serial.println(temp);//調(diào)試用
//Serial.println(bitRead(temp,i));//調(diào)試用
digitalWrite(IIC_SDA,bitRead(temp,i));
//Serial.println(digitalRead(test));
// x=(digitalRead(test))|(x<<1);//調(diào)試用
//Serial.println(x);//調(diào)試用
//Serial.println(i);//調(diào)試用
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對應(yīng)內(nèi)部哪個地址
ACK();
write_byte(date); //所寫入的數(shù)據(jù)
//Serial.println("所寫入的數(shù)據(jù):");
//Serial.println(date);
ACK();
IIC_stop();
//Serial.println("PCA9685_write結(jié)束");
}
//從PCA9685讀數(shù)據(jù)有返回值
unsigned char PCA9685_read(unsigned char address)
{
unsigned char date;
start();
write_byte(PCA9685_adrr); //PCA9685
ACK();
write_byte(address);//PCA9685對應(yīng)內(nèi)部哪個地址
ACK();
start();
write_byte(PCA9685_adrr|0x01); //表示此時狀態(tài)為從PCA9685讀取數(shù)據(jù)
ACK();
date=read_byte();
// Serial.println("date");
//Serial.println(date);
return date;
}
//PCA9685復(fù)位
void reset(void)
{//Serial.println("RESET開始");
PCA9685_write(PCA9685_MODE1,0x0);
//Serial.println("RESET結(jié)束");
}
//PCA9685修改頻率
/*---------------------------------------------------------------
PCA9685修改角度函數(shù)
num:舵機(jī)PWM輸出引腳0~15,on:PWM上升計數(shù)值0~4096,off:PWM下降計數(shù)值0~4096
一個PWM周期分成4096份,由0開始+1計數(shù),計到on時跳變?yōu)楦唠娖?繼續(xù)計數(shù)到off時
跳變?yōu)榈碗娖?直到計滿4096重新開始。所以當(dāng)on不等于0時可作延時,當(dāng)on等于0時,
off/4096的值就是PWM的占空比。
----------------------------------------------------------------*/
void setPWMFreq(float freq)
{ //Serial.print("設(shè)置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("周期頻率設(shè)置值:");
// 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("當(dāng)前通道:");
//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路舵機(jī)從60度轉(zhuǎn)到0角度
}
else
{
stage1_ctrl=true;
setPWM(0, 0,224); //第0路舵機(jī)從0度轉(zhuǎn)到60角度
}
//setPWM(0, 0, SERVO180);//第0路舵機(jī)轉(zhuǎn)到0角度
//setPWM(1, 0, SERVO000);//第1路舵機(jī)轉(zhuǎn)到0角度
}
*/
復(fù)制代碼
作者:
admin
時間:
2018-8-25 17:02
原理圖能分享下嗎?
作者:
Riton
時間:
2018-8-26 11:55
admin 發(fā)表于 2018-8-25 17:02
原理圖能分享下嗎?
額,沒有畫原理圖,直接接線的。我等下簡單手畫一下吧
作者:
Riton
時間:
2018-8-26 12:48
不懂得怎么發(fā)圖片啊。。。那樣成功了沒
作者:
stu
時間:
2018-11-8 19:18
Riton 發(fā)表于 2018-8-26 12:48
不懂得怎么發(fā)圖片啊。。。那樣成功了沒
沒有看到圖片
歡迎光臨 (http://www.torrancerestoration.com/bbs/)
Powered by Discuz! X3.1