机器人程序.docx
- 文档编号:30492807
- 上传时间:2023-08-15
- 格式:DOCX
- 页数:41
- 大小:17.78KB
机器人程序.docx
《机器人程序.docx》由会员分享,可在线阅读,更多相关《机器人程序.docx(41页珍藏版)》请在冰豆网上搜索。
机器人程序
#include
sbitl_wheel_h=P0^3;//左轮前进
sbitl_wheel_b=P0^4;//左轮后退
sbitr_wheel_h=P0^6;//右轮前进
sbitr_wheel_b=P0^5;//右轮后退
sbitl_wheel_slow=P0^2;//左轮慢速档
sbitr_wheel_slow=P0^7;//右转慢速档
sbitput_res=P2^6;//释放物品
sfrsensor_in=0x90;
unsignedcharbdatasensor_state;
sbitrh_sensor=sensor_state^0;//巡线传感器前排的左边
sbitmh_sensor=sensor_state^1;//巡线传感器前排的中间
sbitlh_sensor=sensor_state^2;//巡线传感器前排的右边
sbitlb_sensor=sensor_state^4;//巡线传感器后排的左边
sbitmb_sensor=sensor_state^5;//巡线传感器后排的中间
sbitrb_sensor=sensor_state^6;//巡线传感器后排的右边
sbitlcoun_sensor=sensor_state^3;//巡线传感器计数器的左
sbitrcoun_sensor=sensor_state^7;//巡线传感器计数器的右
bith_flag,b_flag,slow,speedy;
bitmidst,left,right,last;
bitintersection,decussation;
bitstop_f,begin_f,st_flag;
unsignedcharcrossing,time,irq_times;
unsignedintraster;
unsignedcharplace_num;
unsignedintsys_time;
sbitchoose0=P3^2;
sbitchoose1=P3^3;
sbitchoose2=P3^4;
sbitstart_azimuth=P3^1;
sbitcome_in_k=P3^0;
sbitstart=P3^6;
/********************************************************************************************************/
//定时器0中断
/************************************/
//定时50ms
/***********************************/
time0(void)interrupt1using1
{
TH0=0x3c;
TL0=0xa8;
time++;
if(st_flag)
{
if(++irq_times>=20)
{
sys_time++;
irq_times=0;
}
}
}
/********************************************************************************************************/
//延时子程序
voiddelay(x,y,z)
unsignedcharx,y,z;
{
unsignedchari,j,k;
for(i=x;i>0;i--)
for(j=y;j>0;j--)
for(k=z;k>0;k--);
}
/*******************************************************************************************************/
//巡线
voidcruise(void)
{
sensor_state=sensor_in;
intersection=rh_sensor|mh_sensor|lh_sensor;
if(intersection)
{
if((!
lcoun_sensor)&&(!
rcoun_sensor)&&decussation)
{
crossing++;
decussation=0;
TH1=0;
TL1=0;
raster=0;
}
raster=TH1*256+TL1;
if(raster>2200)
{
crossing++;
decussation=1;
TH1=0;
TL1=0;
}
if(!
mh_sensor)
{
delay(1,1,255);
if(!
mh_sensor)
{
left=0;
right=0;
midst=1;
if(slow)
{
l_wheel_h=0;
r_wheel_h=0;
l_wheel_slow=0;
r_wheel_slow=0;
}
else
{
l_wheel_h=0;
r_wheel_h=0;
l_wheel_slow=1;
r_wheel_slow=1;
}
}
}
elseif(!
rh_sensor)
{
delay(1,1,255);
if(!
rh_sensor)
{
right=1;
midst=0;
last=1;
}
}
elseif(!
lh_sensor)
{
delay(1,1,255);
if(!
lh_sensor)
{
left=1;
midst=0;
last=0;
}
}
if(right&&last)
{
if(slow)
{
r_wheel_h=1;
l_wheel_h=0;
}
else
{
l_wheel_slow=1;
r_wheel_slow=0;
}
}
if(left&&!
last)
{
if(slow)
{
r_wheel_h=0;
l_wheel_h=1;
}
else
{
l_wheel_slow=0;
r_wheel_slow=1;
}
}
}
else
{
decussation=1;
if(!
mb_sensor)
{
delay(1,1,255);
if(!
mb_sensor)
{
if(slow)
{
l_wheel_h=0;
r_wheel_h=0;
l_wheel_slow=0;
r_wheel_slow=0;
}
else
{
l_wheel_h=0;
r_wheel_h=0;
l_wheel_slow=1;
r_wheel_slow=1;
}
}
}
elseif(!
lb_sensor)
{
delay(1,1,255);
if(!
lb_sensor)
{
left=1;
midst=0;
last=0;
}
}
elseif(!
rb_sensor)
{
delay(1,1,255);
if(!
rb_sensor)
{
right=1;
midst=0;
last=1;
}
}
if(left&&!
last)
{
if(slow)
{
l_wheel_h=1;
r_wheel_h=0;
}
else
{
l_wheel_slow=0;
r_wheel_slow=1;
}
}
if(right&&last)
{
if(slow)
{
l_wheel_h=0;
r_wheel_h=1;
}
else
{
l_wheel_slow=1;
r_wheel_slow=0;
}
}
}
}
/***********************************************************************************************/
//起动
voidgo(void)
{
l_wheel_h=0;
r_wheel_h=0;
l_wheel_b=1;
r_wheel_b=1;
}
/***********************************************************************************************/
//停止
voidstop(void)
{
l_wheel_h=1;
r_wheel_h=1;
l_wheel_b=1;
r_wheel_b=1;
}
/***********************************************************************************************/
//后退
voidback(unsignedchart)
{
l_wheel_h=0;
r_wheel_h=0;
l_wheel_b=0;
r_wheel_b=0;
time=0;
while(time l_wheel_h=1; r_wheel_h=1; l_wheel_b=1; r_wheel_b=1; } /***********************************************************************************************/ //速度选择 voidspeed_chose(unsignedchars) { switch(s) { case'h': { l_wheel_slow=1; r_wheel_slow=1; slow=0; }break; case'l': { l_wheel_slow=0; r_wheel_slow=0; slow=1; }break; } } /***********************************************************************************************/ //右转90度 voidturn_right(void) { bittemp_b; stop(); time=0; while(time<2); l_wheel_h=0; l_wheel_b=1; r_wheel_h=0; r_wheel_b=0; speed_chose('l'); if(rh_sensor) { time=0; do { sensor_state=sensor_in; if(! mh_sensor)break; } while(lh_sensor); if(time<20)temp_b=1; if(temp_b) { do sensor_state=sensor_in; while(rh_sensor||! mh_sensor); } } elseif(lh_sensor) { do sensor_state=sensor_in; while(lh_sensor); do sensor_state=sensor_in; while(rh_sensor||! mh_sensor); } else { time=0; while(time<20); } do sensor_state=sensor_in; while(mh_sensor); stop(); time=0; while(time<2); sensor_state=sensor_in; while(mh_sensor&&lh_sensor&&rh_sensor) { sensor_state=sensor_in; if(mh_sensor&&rh_sensor&&lh_sensor) { l_wheel_h=0; l_wheel_b=0; r_wheel_h=0; r_wheel_b=1; do sensor_state=sensor_in; while(mh_sensor); stop(); time=0; while(time<2); } sensor_state=sensor_in; if(mh_sensor&&rh_sensor&&lh_sensor) { l_wheel_h=0; l_wheel_b=1; r_wheel_h=0; r_wheel_b=0; do sensor_state=sensor_in; while(mh_sensor); stop(); time=0; while(time<2); } } go(); TH1=0; TL1=0; } /***********************************************************************************************/ //左转90度 voidturn_left(void) { unsignedchartemp_b; stop(); time=0; while(time<2); l_wheel_h=0; l_wheel_b=0; r_wheel_h=0; r_wheel_b=1; speed_chose('l'); if(lh_sensor) { time=0; do { sensor_state=sensor_in; if(! mh_sensor)break; } while(rh_sensor); if(time<20)temp_b=1; if(temp_b) { do sensor_state=sensor_in; while(lh_sensor||! mh_sensor); } } elseif(rh_sensor) { do sensor_state=sensor_in; while(rh_sensor); do sensor_state=sensor_in; while(rh_sensor||! mh_sensor); } else { time=0; while(time<20); } do sensor_state=sensor_in; while(mh_sensor); stop(); time=0; while(time<2); sensor_state=sensor_in; while(mh_sensor&&lh_sensor&&rh_sensor) { sensor_state=sensor_in; if(mh_sensor&&rh_sensor&&lh_sensor) { l_wheel_h=0; l_wheel_b=1; r_wheel_h=0; r_wheel_b=0; do sensor_state=sensor_in; while(mh_sensor); stop(); time=0; while(time<2); } sensor_state=sensor_in; if(mh_sensor&&rh_sensor&&lh_sensor) { l_wheel_h=0; l_wheel_b=0; r_wheel_h=0; r_wheel_b=1; do sensor_state=sensor_in; while(mh_sensor); stop(); time=0; while(time<2); } } go(); TH1=0; TL1=0; } /***********************************************************************************************/ //转过一个角度 voidturn_angle(unsignedcharderi,unsignedintangle) { stop(); time=0; while(time<4); switch(deri) {case'l': {l_wheel_h=0; l_wheel_b=0; r_wheel_h=0; r_wheel_b=1; l_wheel_slow=0; r_wheel_slow=0; TH1=0; TL1=0; raster=0; while(raster l_wheel_slow=1; r_wheel_slow=1; stop(); }break; case'r': {l_wheel_h=0; l_wheel_b=1; r_wheel_h=0; r_wheel_b=0; l_wheel_slow=0; r_wheel_slow=0; TH1=0; TL1=0; raster=0; while(raster l_wheel_slow=1; r_wheel_slow=1; stop(); }break; TH1=0; TL1=0; } } voidbegin(unsignedchardirection) { switch(direction) { case'l': { speed_chose('l'); go(); crossing=0; do { sensor_state=sensor_in; if((! rh_sensor)&&(! mh_sensor)&&(! lh_sensor)) { decussation=1; } elseif((! lcoun_sensor)&&(! rcoun_sensor)&&decussation) { crossing++; decussation=0; } } while(crossing<2); time=0; while(time<2); l_wheel_h=1; time=0; while(time<14) { sensor_state=sensor_in; if(! lh_sensor)break; } l_wheel_h=0; do { sensor_state=sensor_in; if(! rh_sensor)break; } while(mh_sensor); go(); crossing=0; TH1=0; TL1=0; do cruise(); while(crossing<2); speed_chose('h'); crossing=2; TH1=0; TL1=0; }break; case'r': { speed_chose('l'); go(); crossing=0; do { sensor_state=sensor_in; if((! rh_sensor)&&(! mh_sensor)&&(! lh_sensor)) { decussation=1; } elseif((! lcoun_sensor)&&(! rcoun_sensor)&&decussation) { crossing++; decussation=0; } } while(crossing<2); time=0; while(time<2); r_wheel_h=1; time=0; while(time<14) { sensor_state=sensor_in; if(! rh_sensor)break; } r_wheel_h=0; do { sensor_state=sensor_in; if(! lh_sensor)break; } while(mh_sensor); go(); crossing=0; TH1=0; TL1=0; do cruise(); while(crossing<2); turn_left(); crossing=0; TH1=0; TL1=0; do cruise(); while(crossing<2); turn_right(); crossing=0; TH1=0; TL1=0; do cruise(); while(crossing<2); speed_chose('h'); crossing=4; TH1=0; TL1=0; }break; } } voidjettison(void) { bittemp_b; unsignedchari; switch(place_num) { case10: { if(crossing==7) { speed_chose('l'); } elseif(crossing==8) { turn_right(); crossing=10; } if(crossing>=10) { if(! come_in_k) { time=0; while(time<10); stop_f=1; stop(); time=0; while(time<240) { if(time%8) put_res=1; else put_res=0; } put_res=1; } } }break; case11: { i
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 机器人 程序