📄 robot_for_lines.c
字号:
#include<c8051f020.h>
#include <stdio.h>
//*****************************************************//
sbit cl1=P1^4;//..........左辨色
sbit cl2=P1^5;
sbit cl3=P1^6;
sbit cl4=P1^7;
sbit cr1=P2^4;//..........右辨色
sbit cr2=P2^5;
sbit cr3=P2^6;
sbit cr4=P2^7;
sbit left1=P0^6;//..............................左滑道电机C1
sbit left2=P0^7;//..............................C2
sbit right1=P3^0;//.............................右滑道电机C1
sbit right2=P3^1;//.............................C2
sbit leftup=P0^4;//.............................左肢上升C1
sbit leftdown=P0^5;//............................左肢下降C2
sbit rightup=P3^2;//............................右肢上升C1
sbit rightdown=P3^3;//..........................右肢下降C2
sbit lstop=P1^3;//.....................左干簧管
sbit rstop=P2^3;//.....................右干簧管
sbit duishe=P2^2;//....................对射停车
sbit lock1_l=P0^3;
sbit lock2_l=P0^2;
sbit lock1_r=P3^7;
sbit lock2_r=P3^6;
//*******************************************************//
int a=1,t=1;
int m=7;
int codel1,codel2,codel3,codel4,coder1,coder2,coder3,coder4;
int lu=1,ld=1,ru=1,rd=1;//.....................四电机控制量
//*******************************************//
sbit CP_left=P0^0; //脉冲
sbit CP_right=P3^4;
sbit DIR_left=P0^1; //转向
sbit DIR_right=P3^5;
sbit front_left1=P1^0; //巡线光电管
sbit front_left2=P1^1;
sbit front_right1=P2^0;
sbit front_right2=P2^1;
//***************************************************//
void port_init(void);
void int_init(void);
void delay(int,int);
void TIMER0(void);
void TIMER1(void);
void TIMER2(void);
void TIMER3(void);
void keep_speed(int,int,int);
void decelerate(int,int,int,int);
void accelerate(int,int,int,int);
void turn_left(int,int,int,int);
void search_line(int);
void turn_right(int,int,int,int);
void stop (void);
void rise(void);
//**********************************************//
int vi,vt,vi0,vi1,vt,v; //定义初始化值
//**********************************************//
int i,j,c,k,z=0;
int f_light_right1,f_light_right2,f_light_left1,f_light_left2; // OOOO
int last_f_left1,last_f_left2,last_f_right1,last_f_right2; // 1212
//**********************************************//
void l_headleft(void);
void l_headright(void);
void r_headleft(void);
void r_headright(void);
void tou_l(void); //先停 投右
void tou_r(void); //先停 投左
void stop_l(void);
void stop_r(void);
void down_l(void);
void down_r(void);
void rise(void);
void stop_ctrl(void);
void again(void);
void main()
{
WDTCN=0xde;
WDTCN=0xad;
port_init();
int_init();
delay(50,300);
left1=1; //初始时四电机均处停状态
left2=1;
right1=1;
right2=1;
leftup=1;
leftdown=1;
rightup=1;
rightdown=1;
lock1_l=1;
lock2_l=1;
lock1_r=1;
lock2_r=1;
DIR_left=0; //设置转向
DIR_right=1;
TH0=0xff; //预先赋一次值
TL0=0x30;
TH1=0xff;
TL1=0x30;
TH2=0xff;
TL2=0xb0;
PT2=1; //设置优先级
EA=1; //开中断
ET0=1;
TR0=1;
ET1=1;
TR1=1;
f_light_right1=front_right1;
f_light_right2=front_right2;
f_light_left1=front_left1;
f_light_left2=front_left2;
//start TIMER2
accelerate(0x30,0xe0,1,80);
rise();
TMR3CN |= 0x04; // start Timer3
z=0;
ET2=1; //enable interrupt TIMER2
TR2=1;
while(a==1)
{
while(z<=10800) //由z的值控制巡线长度
{
search_line(0xe0);
}
decelerate(0x60,0xe0,1,9000);
while(t==1)
{
while(duishe!=1);
delay(100,10);
if(duishe==1)
{
stop();
t=0;
a=0;
}
}
}
delay(100,4000);
while(1)//..................................................排除辨色灯初始辨别时的误脉冲
{
codel1=cl1;
codel2=cl2;
codel3=cl3;
codel4=cl4;
coder1=cr1;
coder2=cr2;
coder3=cr3;
coder4=cr4;
delay(100,5);
if(cl1==codel1&cl2==codel2&cl3==codel3&cl4==codel4&cr1==coder1&cr2==coder2&cr3==coder3&cr4==coder4)
break;
}
//************情形1：左右手臂分别为红，红；或红绿，红时。左肢左行至绿然后投，右肢直接投***************//
if(codel3==1&codel4==1&coder3==1&coder4==1)//第二四列灯为红时
{
l_headleft();
tou_r();
while(1)
{
if((cl3==0|cl4==0)&ld==1)
{
delay(100,5);
if(cl3==0|cl4==0)
{
tou_l();
ld=0;
}
}
stop_ctrl();
if(lu==0&ru==0)
break;
}
again();
}
//*************************情形2：中间四灯处于红绿边界时，左右肢都直接投*************************//
else if(codel3==0&codel4==0&coder1==1&coder2==1)
{
tou_l();
tou_r();
while(1)
{
stop_ctrl();
if(lu==0&ru==0)
break;
}
again();
}
//*********************情形3：右四灯为红兰边界时，左肢左行至绿，右肢左行至红*************************//
else if(coder1==1&coder2==1&coder3==0&coder4==1)
{
l_headleft();
r_headleft();
while(1)
{
if((cl3==0|cl4==0)&ld==1)
{
delay(100,5);
if(cl3==0|cl4==0)
{
tou_l();
ld=0;
}
}
if(cr3==1&rd==1)
{
delay(100,5);
if(cr3==1)
{
tou_r();
rd=0;
}
}
stop_ctrl();
if(lu==0&ru==0)
break;
}
again();
}
//*******************情形4：左右肢分别在绿，红绿；或绿，绿时，左肢直接投，右肢右行至红投*************//
else if(codel1==0&codel2==0&coder1==0&coder2==0)//一三列灯为绿时
{
tou_l();
r_headright();
while(1)
{
if((cr1==1|cr2==1)&rd==1)
{
delay(100,5);
if(cr1==1|cr2==1)
{
tou_r();
rd=0;
}
}
stop_ctrl();
if(lu==0&ru==0)
break;
}
again();
}
//******************情形5：左肢为绿兰时，左肢右行至绿投，右肢右行至红投*******************************//
else if(codel1==0&codel2==1&codel3==0&codel4==0)//一三四列分别为兰,绿,绿时
{
l_headright();
r_headright();
while(1)
{
if(cl2==0&ld==1)
{
delay(100,10);
if(cl2==0)
{
tou_l();
ld=0;
}
}
if((cr1==1|cr2==1)&rd==1)
{
delay(100,5);
if(cr1==1|cr2==1)
{
tou_r();
rd=0;
}
}
stop_ctrl();
if(lu==0&ru==0)
break;
}
again();
}
//****************情景6：中间四灯为兰绿时，左肢左行至红，右肢直接投***********************************//
else if(codel3==0&codel4==1&coder1==0&coder2==0)
{
l_headleft();
tou_r();
while(1)
{
if(cl3==1&ld==1)
{
delay(100,5);
if(cl3==1)
{
tou_l();
ld=0;
}
}
stop_ctrl();
if(lu==0&ru==0)
break;
}
again();
}
//*****************情形7：中间四灯为兰时，左肢左行至红投，右肢右行至绿投******************************//
else if(codel3==0&codel4==1&coder1==0&coder2==1)
{
l_headleft();
r_headright();
while(1)
{
if(cl3==1&ld==1)
{
delay(100,5);
if(cl3==1)
{
tou_l();
ld=0;
}
}
if(cr2==0&rd==1)
{
delay(100,5);
if(cr2==0)
{
tou_r();
rd=0;
}
}
stop_ctrl();
if(lu==0&ru==0)
break;
}
again();
}
//*******************情形8：中间四灯为红兰边界时，左肢直接投，右肢右行至绿投*****************************//
else if(codel3==1&codel4==1&coder1==0&coder2==1)
{
tou_l();
r_headright();
while(1)
{
if(cr2==0&rd==1)
{
delay(100,5);
if(cr2==0)
{
tou_r();
rd=0;
}
}
stop_ctrl();
if(lu==0&ru==0)
break;
}
again();
}
while(1);
}
//**************************************************//
void delay(int time,int number) //延迟程序
{ int i,j;
for(i=0;i<time;i++)
for(j=0;j<number;j++);
}
//**************************************************//
void port_init(void) //端口初始化程序
{ OSCICN= 0x07; //16M时钟12分频
XBR2=0x40; //设置交叉开关
P0MDOUT |=0xff;
P3MDOUT |=0xff;
TMOD=0x11; //设置定时器0,1，方式为01
T2CON=0x01; //定时器2工作方式1，16位捕捉
}
//*********************************************//
//中断初始化
void int_init(void)
{
TMR3CN = 0x00; // Stop Timer3; Clear TF3;
TMR3RLL=0x00;
TMR3RLH=0x0f;
EIE2 |= 0x01; // enable Timer3 interrupts
}
//**************************************************//
void TIMER0(void) interrupt 1 using 0 //定时器0中断
{ TH0=0xff;
TL0=vi0;
CP_right=~CP_right;
}
void TIMER1(void) interrupt 3 using 1 //定时器1中断
{ TH1=0xff;
TL1=vi1;
CP_left=~CP_left;
}
void TIMER2(void) interrupt 5 using 3 //定时器2中断
{ TF2=0; //千万不能少，硬件不能自动清0
f_light_right1=front_right1;
f_light_right2=front_right2;
f_light_left1=front_left1;
f_light_left2=front_left2;
TH2=0xff;
TL2=0x99;
z++;
}
//**************************************************//
//中断服务程序
void TIMER3() interrupt 14 using 2
{
TMR3CN &= ~(0x80); // clear TF3
m--;
if (m==1)
{
lock2_l=0;
lock2_r=0;
TMR3CN=0; //上升到位后关掉timer3
}
else if(m==4)
{
leftup=1;
rightup=1;
}
}
//**************************************************//
void accelerate(int vo,int vt,int b,int t) //加速到v开始循线
{
int dt=1;
vi=vo;
for(k=0;k<=300;k++)
{ delay(2,t);
vi+=b;
vi0=vi;
vi1=vi;
dt++;
t=t+dt;
if(vi==vt) break;
}
}
//**************************************************//
void keep_speed(int vt,int number,int time) //匀速程序
{ vi=vt;
vi0=vi;
vi1=vi;
for(c=0;c<=number;c++)
{ delay(50,time);
}
}
//**************************************************//
void decelerate(int vo,int vt,int d,int t) //减速到由vt到v循线
{
int dt=(vt-vo)/d;
vi=vt;
for(k=0;k<=300;k++)
{ delay(1,t);
vi-=d;
vi0=vi;
vi1=vi;
dt--;
t-=dt;
if(vi==vo)
break;
}
}
//************************************************//
void stop() //停车
{
EA=0;
TR0=0;
TR1=0;
}
//**************************************************/
void turn_left(int number,int time,int sub,int add) //左转弯
{ for(i=0;i<=number;i++)
{ delay(10,time);
vi1-=sub;
vi0+=add;
}
for(i=0;i<=number;i++)
{ delay(10,time);
vi1+=sub;
vi0-=add;
}
}
//**************************************************/
void turn_right(int number,int time,int sub,int add) //右转弯
{ for(i=0;i<=number;i++)
{ delay(10,time);
vi0-=sub;
vi1+=add;
}
for(i=0;i<=number;i++)
{ delay(10,time);
vi0+=sub;
vi1-=add;
}
}
//***************************************************//
void search_line(int vt) //循线程序
{
if((f_light_left1==1)&&(f_light_left2==0)&&(f_light_right1==1)&&(f_light_right2==1)) //1011
turn_left(1,1,2,0);
else if((f_light_left1==0)&&(f_light_left2==0)&&(f_light_right1==1)&&(f_light_right2==1)) //0011
turn_left(1,2,2,0);
else if((f_light_left1==0)&&(f_light_left2==1)&&(f_light_right1==1)&&(f_light_right2==1)) //0111
turn_left(2,5,2,0);
else if((f_light_left1==1)&&(f_light_left2==1)&&(f_light_right1==0)&&(f_light_right2==1)) //1101
turn_right(1,1,2,0);
else if((f_light_left1==1)&&(f_light_left2==1)&&(f_light_right1==0)&&(f_light_right2==0)) //1100
turn_right(1,2,2,0);
else if((f_light_left1==1)&&(f_light_left2==1)&&(f_light_right1==1)&&(f_light_right2==0)) // 1110
turn_right(2,5,2,0);
else if((f_light_left2==0)&&(f_light_right1==0)) // X00X
keep_speed(vt,2,2);
else if((f_light_left1==1)&&(f_light_left2==1)&&(f_light_right1==1)&&(f_light_right2==1)) //1111
{ if((last_f_left1==1)&&(last_f_left2==1)&&(last_f_right1==1)&&(last_f_right2==0)) //LAST 1110
for(;;)
{
turn_right(2,2,2,0);
if((front_left2==0)&&(front_right1==0))
break;
}
if((last_f_left1==0)&&(last_f_left2==1)&&(last_f_right1==1)&&(last_f_right2==1)) //LAST 0111
for(;;)
{
turn_left(2,2,2,0);
if((front_left2==0)&&(front_right1==0))
break;
}
}
else keep_speed(vt,3,3);
last_f_left1=f_light_left1;
last_f_left2=f_light_left2;
last_f_right1=f_light_right1;
last_f_right2=f_light_right2;
}
//**********************************************//
void l_headleft(void)
{
left1=1;
left2=0;
}
void l_headright(void)
{
left1=0;
left2=1;
}
void r_headleft(void)
{
right1=1;
right2=0;
}
void r_headright(void)
{
right1=0;
right2=1;
}
void tou_l(void)
{
left1=1;
left2=1;
leftup=0;
leftdown=1;
lock2_l=1;
}
void tou_r(void)
{
right1=1;
right2=1;
rightup=0;
rightdown=1;
lock2_r=1;
}
void stop_l(void)
{
leftup=1;
leftdown=1;
}
void stop_r(void)
{
rightup=1;
rightdown=1;
}
void down_l(void)
{
leftup=1;
leftdown=0;
}
void down_r(void)
{
rightup=1;
rightdown=0;
}
void stop_ctrl(void)
{
if(lstop==0&lu==1)
{
delay(100,10);
if(lstop==0)
{
stop_l();
lu=0;
}
}
if(rstop==0&ru==1)
{
delay(100,10);
if(rstop==0)
{
stop_r();
ru=0;
}
}
}
void again(void)
{
int n;
for(n=0;n<1;n++)
{
lu=1;
ru=1;
delay(100,30000);
down_l();
down_r();
delay(100,300);
stop_l();
stop_r();
delay(100,400);
lock2_l=0;
lock2_r=0;
delay(100,30000);
tou_l();
tou_r();
while(1)
{
stop_ctrl();
if(lu==0&ru==0)
break;
}
delay(300,30000);
down_l();
down_r();
delay(100,300);
stop_l();
stop_r();
delay(100,400);
lock2_l=0;
lock2_r=0;
}
}
void rise(void)
{
leftup=0;
rightup=0;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -