|
|
#include "reg52.h"
#include "main.h"
#include "distance_measure.h"
#define TIMER_DELAY (500) //定時器定時500us=0.5ms
uint counter = 0; //計數(shù)
uchar PWM[4]={8,8,8,8};
uchar motor_max=5; //pwm最大值
//四路馬達(dá)
// 1 3
// 2 4
sbit ENA = P1^5;
sbit motor1 = P1^4;
sbit motor2 = P1^3;
sbit motor3 = P1^2;
sbit motor4 = P1^1;
sbit ENB = P1^0;
//STC89系列單片機(jī)延時1ms
void delay_ms(unsigned int x)
{
char j=0;
while(x--)
for(j=0;j<125;j++)
;
}
//定時器初始化
void timer_init()
{
//AUXR &= 0x7F; //定時器時鐘12T模式
TMOD &= 0xF0; //設(shè)置定時器模式：定時器0方式1
TMOD |= 0x01; //設(shè)置定時器模式：定時器0方式1
EA=1; //開總中斷
TH0=(65535-TIMER_DELAY)/256; //設(shè)置初值
TL0=(65535-TIMER_DELAY)%256;
ET0=1; //啟用定時器中斷
TR0=1; //開啟
}
//T0中斷服務(wù)子程序
void timer0_isr() interrupt 1
{
TH0=(65535-TIMER_DELAY)/256; //重新賦值
TL0=(65535-TIMER_DELAY)%256;
counter ++;
//計數(shù)累加1ms清零
if(counter == 20)
{
counter = 0;
}
//比較輸出PWM
motor1 = PWM[0] > counter;
motor2 = PWM[1] > counter;
motor3 = PWM[2] > counter;
motor4 = PWM[3] > counter;
}
void motor_up() //前進(jìn)
{
PWM[0]=motor_max;
PWM[1]=0;
PWM[2]=motor_max;
PWM[3]=0;
}
void motor_stop() //后退
{
PWM[0]=0;
PWM[1]=motor_max;
PWM[2]=0;
PWM[3]=motor_max;
}
void motor_left() //左轉(zhuǎn)
{
PWM[0]=motor_max;
PWM[1]=0;
PWM[2]=0;
PWM[3]=0;
}
void motor_right() //右轉(zhuǎn)
{
PWM[0]=0;
PWM[1]=0;
PWM[2]=motor_max;
PWM[3]=0;
}
void main()
{
timer_init(); //定時器初始化
ENA=1;
ENB=1;
motor_up();
while(1)
{
if(get_distance() <= 50) //判斷是否有障礙物
{
delay_ms(10); //消抖
if(get_distance() <= 50) //確實有障礙物
{
motor_left();
delay_ms(500);
}
}
else
{
motor_up();
}
}
}
|
|
