|
|
#include "motor.h"
#include "uart.h"
#include "timer.h"
#include "mem.h"
uchar Cruising_Flag =0x00;
uchar Robots_Run_Status;
uchar Pre_Cruising_Flag = 0x00;
extern uint16 se_timer[8];
/**電機初始化**/
void Motor_Init(void)
{
MOTOR_A_EN=1;
MOTOR_B_EN=1;
MOTOR_GO_STOP;
}
//跟隨模式
void Follow_Track(void)
{
switch(Robots_Run_Status)
{
case 0x01:MOTOR_GO_RIGHT; break;
case 0x02:MOTOR_GO_LEFT; break;
case 0x03:MOTOR_GO_FORWARD; break;
case 0x04:MOTOR_GO_STOP; break;
}
if(Input_Detect1 == 1) //中間傳感器OK
{
if((Input_Detect0 == 0)&& (Input_Detect2 == 0)) //兩邊同時探測到障礙物
{
Robots_Run_Status=0x04;//停止
}
if((Input_Detect0 == 0)&& (Input_Detect2 == 1))//左側(cè)障礙物
{
Robots_Run_Status=0x01;//右轉(zhuǎn)
}
if((Input_Detect0 == 1)&& (Input_Detect2 == 0))//右側(cè)障礙物
{
Robots_Run_Status=0x02;//左轉(zhuǎn)
}
if((Input_Detect0 == 1)&& (Input_Detect2 == 1))//無任何障礙物
{
Robots_Run_Status=0x03;//直行
}
}
else
{
Robots_Run_Status=0x04;
}
}
//巡線模式
void FollowLine(void)
{
switch(Robots_Run_Status)
{
case 0x01:MOTOR_GO_RIGHT; break;
case 0x02:MOTOR_GO_LEFT; break;
case 0x03:MOTOR_GO_FORWARD; break;
case 0x04:MOTOR_GO_STOP; break;
}
if((Input_Detect_LEFT == 0)&& (Input_Detect_RIGHT == 0))//兩邊同時探測到障礙物
{
Robots_Run_Status=0x03;//直行
}
if((Input_Detect_LEFT == 0)&& (Input_Detect_RIGHT == 1))//右側(cè)遇到障礙
{
Robots_Run_Status=0x02;//左轉(zhuǎn)
}
if((Input_Detect_LEFT == 1)&& (Input_Detect_RIGHT == 0))//左側(cè)遇到障礙
{
Robots_Run_Status=0x01;//右轉(zhuǎn)
}
if((Input_Detect_LEFT == 1)&& (Input_Detect_RIGHT == 1))//左右都檢測到，就如視頻中的那樣遇到一道橫的膠帶
{
Robots_Run_Status=0x04;//停止
}
}
//避障模式
void Avoiding(void)
{
switch(Robots_Run_Status)
{
case 0x01:MOTOR_GO_RIGHT; break;
case 0x02:MOTOR_GO_LEFT; break;
case 0x03:MOTOR_GO_FORWARD; break;
case 0x04:MOTOR_GO_STOP; break;
case 0x05:MOTOR_GO_BACK; break;
}
if((Input_Detect_LEFT == 1) || (Input_Detect_RIGHT == 1) || (Input_Detect1==0))
{
Robots_Run_Status=0x04;
}
else //否則電機執(zhí)行前進(jìn)動作
{
Robots_Run_Status=0x03;
}
}
//手臂動作展示
void ARMShow(void)
{
se_timer[0]=100;
Delay_Ms(1000);
se_timer[1]=70;
Delay_Ms(1000);
se_timer[2]=60;
Delay_Ms(1000);
se_timer[3]=50;
Delay_Ms(1000);
se_timer[3]=81;
Delay_Ms(1000);
se_timer[2]=160;
Delay_Ms(1000);
se_timer[1]=120;
Delay_Ms(1000);
se_timer[0]=140;
Delay_Ms(1000);
}
//手臂動作展示
void HeadShow(void)
{
Beep=~Beep;
se_timer[7]=170;
Delay_Ms(1000);
se_timer[7]=0;
Delay_Ms(1000);
}
//發(fā)送超聲波
void Send_wave(void)
{
uint16 i;
Trig = 1;
for(i=0;i<150;i++);
Trig = 0;
}
//獲得距離值
uchar Get_Distance(void)
{
uint32 Distance = 0;
Send_wave();
TH1 = 0;
TL1 = 0;
while(TH1<250 && Echo!= 1);
if(TH1 <= 250) //測距范圍<0.5M
{
TH1 = 0;
TL1 = 0;
while(Echo == 1);
Distance = TH1;
Distance = Distance*256;
Distance = Distance + TL1;
Distance = Distance * 17;
Distance = Distance / 22118;
return (uchar)(Distance&0xFF);
}
}
//通過雷達(dá)避障
void AvoidByRadar(void)
{
if(Get_Distance()<0x0A)//如果雷達(dá)回波數(shù)據(jù)小于10厘米觸發(fā)
{
MOTOR_GO_STOP;
}
else
{
MOTOR_GO_FORWARD;
}
}
void Send_Distance(void)
{
UART_send_byte(0xFF);
UART_send_byte(0x03);
UART_send_byte(0x00);
UART_send_byte(Get_Distance());
UART_send_byte(0xFF);
Delay_Ms(1000);
}
//模式執(zhí)行子函數(shù)，根據(jù)標(biāo)志位進(jìn)行判斷
void Cruising_Mod(void)
{
if(Pre_Cruising_Flag != Cruising_Flag)
{
if(Pre_Cruising_Flag != 0)
{
MOTOR_GO_STOP;
}
Pre_Cruising_Flag = Cruising_Flag;
}
switch(Cruising_Flag)
{
case 0x01:Follow_Track(); break;//跟隨模式
case 0x02:FollowLine(); break;//巡線模式
case 0x03:Avoiding(); break;//避障模式
case 0x04:AvoidByRadar();break;//超聲波壁障模式
case 0x05:ARMShow();break;
case 0x06:HeadShow();break;
default:break;
}
}
void Delay_ForBarrier(uint32 t)
{
uint16 i;
while(t--)
{
for(i=0;i<1050;i++);
}
}
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
