|
|
想做一個(gè)小車，用了三個(gè)超聲波，在小車的前和左右。想通過左右的距離檢測(cè)，控制舵機(jī)左右轉(zhuǎn)動(dòng)。電機(jī)只有一個(gè)，同軸帶動(dòng)兩個(gè)輪子轉(zhuǎn)動(dòng)。前面的兩個(gè)輪子連接舵機(jī)。達(dá)到轉(zhuǎn)向的目的。現(xiàn)在的問題是左右超聲波模塊好像沒有起作用，十分困惑。
#include<reg52.h>
#include <intrins.h>
sbit ECHO_L= P2^1; //超聲波接口定義
sbit TRIG_L = P2^0; //超聲波接口定義
sbit ECHO_R = P2^7; //超聲波接口定義
sbit TRIG_R = P2^6;
sbit ECHO_b=P2^4;
sbit TRIG_b=P2^3; //超聲波接口定義
#define dianji_pwm P2^2 //PWM信號(hào)端
sbit Sevro_moto_pwm= P1^7 ;
sbit IN1=P2^0;
sbit IN2=P2^1;
sbit ENA=P2^2;
//光電管排布 ： 左1 左2 右2 右1
sbit left1=P0^0;//尋跡左1
sbit left2=P0^1;//尋跡左2
sbit right2=P0^2;//尋跡右2
sbit right1=P0^3;//尋跡右1
bit Right_moto_stop=1;
#define dianji_go {IN1=0,IN2=1,ENA=1;} //電機(jī)向前走
#define dianji_back {IN1=1,IN2=0,ENA=1;} //電機(jī)向后走
#define dianji_Stop {ENA=0;} //電機(jī)停轉(zhuǎn)
unsigned char pwm_val_dianji =0;//變量定義
unsigned char push_val_dianji =0;//電機(jī)占空比N/20
unsigned char pwm_val_duoji= 0;//變量定義
unsigned char push_val_duoji =15;//舵機(jī)歸中，產(chǎn)生約，1.5MS 信號(hào)
unsigned int time;
unsigned int timer;
float S1;
float S2;
unsigned int flag;
unsigned int time=0; //時(shí)間變量
unsigned int timer=0; //延時(shí)基準(zhǔn)變量
unsigned char timer1=0; //掃描時(shí)間變量
void delay(unsigned int k) //延時(shí)函數(shù) 11.059mhz下約10ms
{
unsigned int x,y;
for(x=0;x<k;x++)
for(y=0;y<2000;y++);
}
void delay_1ms(unsigned char x) //1ms延時(shí)函數(shù)100ms以內(nèi)可用
{
unsigned char i;
while(x--)
for(i=124;i>0;i--);
}
void Delay40us() //@11.0592MHz
{
unsigned char i;
_nop_();
i = 15;
while (--i);
}
void Conut1(void) //計(jì)算距離
{
TRIG_L= 1; //拉高超聲波模塊觸發(fā)IO
Delay40us();// 延時(shí)20us
TRIG_L= 0; //拉低超聲波模塊觸發(fā)IO
while(ECHO_L==0);//等待超聲波模塊輸出IO拉高
TR0 = 1; //開啟定時(shí)器計(jì)時(shí)
while(ECHO_L==1); //等待超聲波模塊輸出IO拉低
TR0 = 0;
time=(TH0*256+TL0);
S1=time*0.170;
TH0=0;
TL0=0;
}
void Conut2(void) //計(jì)算距離
{
TRIG_R= 1; //拉高超聲波模塊觸發(fā)IO
Delay40us();// 延時(shí)20us
TRIG_R= 0; //拉低超聲波模塊觸發(fā)IO
while(ECHO_R==0);//等待超聲波模塊輸出IO拉高
TR0 = 1; //開啟定時(shí)器計(jì)時(shí)
while(ECHO_R==1); //等待超聲波模塊輸出IO拉低
TR0 = 0;
time=(TH0*256+TL0);
S2=time*0.170;
TH0=0;
TL0=0; //算出來是CM
}
void Conut3(void) //計(jì)算距離
{
TRIG_b= 1; //拉高超聲波模塊觸發(fā)IO
Delay40us();// 延時(shí)20us
TRIG_b= 0; //拉低超聲波模塊觸發(fā)IO
while(ECHO_b==0);//等待超聲波模塊輸出IO拉高
TR0 = 1; //開啟定時(shí)器計(jì)時(shí)
while(ECHO_b==1); //等待超聲波模塊輸出IO拉低
TR0 = 0;
time=(TH0*256+TL0);
S2=time*0.170;
TH0=0;
TL0=0; //算出來是CM
}
/*調(diào)節(jié)push_val_duoji的值改變舵機(jī)角度,占空比 */
void pwm_Servomoto(void)
{
if(pwm_val_duoji<=push_val_duoji)
Sevro_moto_pwm=1;
else {Sevro_moto_pwm=0;}
if(pwm_val_duoji>=200)
pwm_val_duoji=0;
}
/*調(diào)節(jié)電機(jī)占空比*/
void pwm_out_dianji(void)
{
if(pwm_val_dianji<=push_val_dianji)
{
ENA=1;
}
else
{
ENA=0;
}
if(pwm_val_dianji>=20)
pwm_val_dianji=0;
else
{
ENA=0;
}
}
void COMM(void)
{
Conut1();
Conut2();
if(S3<20)
{
ENA=0;;
if(S1<10&&S2>10) flag=0;
if(S2<10&&S1>10) flag=1;
if(S1<10&&S2<10) flag=2;
if(S2>10&&S1>10) flag=3;
switch(flag)
{
case 0 :push_val_duoji=17; break;
case 1 :push_val_duoji=11; break;
case 2 :push_val_duoji=14; break;
case 3 :push_val_duoji=14; break;
}}
}
void COMM1()
{
}
/***************************************************/
void main(void)
{
TMOD=0X11;
TH1=(65536-100)/256; //100US定時(shí)
TL1=(65536-100)%256;
TR1= 1;
ET1= 1;
EA = 1;
push_val_duoji=15; //舵機(jī)歸中
delay();
while(1) //無限循環(huán)
{
COMM();
}
}
///*TIMER1中斷服務(wù)子函數(shù)產(chǎn)生PWM信號(hào)*/
void timer0()interrupt 1
{
}
void time1()interrupt 3
{
TH1=(65536-100)/256; //100US定時(shí)
TL1=(65536-100)%256;
timer++; //定時(shí)器100US為準(zhǔn)。在這個(gè)基礎(chǔ)上延時(shí)
pwm_val_duoji++;
pwm_out_dianji();
pwm_Servomoto();
}
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
