/****机械手在工位1、2处分别抓球一次项目程序(使用端口7、8)*******/

/***项目接线表***/

/* 物料搬运装置 传感器模块 主机模块

4 IN0 OUT0 P1.0

5 IN1 OUT1 P1.1

6 IN2 OUT2 P1.2

7 IN3 OUT3 P1.3

8 IN4 OUT4 P1.4

9 IN5 OUT5 P1.5

物料搬运装置 继电器模块 主机模块

13 KA1 P3.0

14 KA2 P3.1

15 KA3 P3.2

16 KA4 P3.3

17 KA5 P3.4 

*/

#include

#define uint unsigned int

#define uchar unsigned char

uchar num=0,num1=0,num2=0,num3=0,a=0,num4=0,num5=0;

/*****信号检测口***********/

sbit dk4=P1^0;//工位1

sbit dk5=P1^1;//工位2

sbit dk6=P1^2;//工位3

sbit dk7=P1^3;//上升到位

sbit dk8=P1^4;//下降到位

sbit dk9=P1^5;//抓紧到位

sbit dk10=P1^6;//抓手是否有物料

/*****控制端口**************/

sbit dk13=P3^0;//左移

sbit dk14=P3^1;//右移

sbit dk15=P3^2;//抓紧

sbit dk16=P3^3;//放松

sbit dk17=P3^4;//上下 #define jxs_sd dk13=1;dk14=1; //电机停止

#define jxs_zy dk13=0;dk14=1;//左移

#define jxs_yy dk13=1;dk14=0;//右移

#define jxs_zj dk15=0;dk16=1;//抓紧 

#define jxs_fs dk15=1;dk16=0;//放松

#define jxs_bc dk15=1;dk16=1;//抓手保持

#define jxs_ss dk17=1;//上升

#define jxs_xj dk17=0;//下降

void cx(cxms) //延时子程序

{

uchar a,b;

for(a=210;a>0;a--)

for(b=cxms;b>0;b--);

}

void jxs_gw2()//到达工位2（为在工位2抓球做准备）

{

while(num2)

{

if(dk5==0)

{

jxs_sd;

cx(100);

num=1;

num2=0;

}

}

}

void jxs_gw3()//机械手到工位3放球

{

while(num2) 

{

if(dk7==0) //当机械手上升到位后才能进行左右移动

{

jxs_zy;//机械手左移 

if(dk6==0)//当机械手到达工位3就执行以下程序

{

jxs_sd;//电机停止移动

cx(100);

jxs_fs; //抓手释放球

num2=0;

num3=1;

}

}

}

}

void jxs_gw33()//机械手到工位3放球

{

while(num5) 

{

if(dk7==0) //当机械手上升到位后才能进行左右移动

{

jxs_zy;//机械手左移 

if(dk6==0)//当机械手到达工位3就执行以下程序

{

jxs_sd;//电机停止移动

cx(100);

jxs_fs; //抓手释放球

num5=0;

num=1;

}

}

}

}

void jxs_zq()//机械手下降抓球

{

while(num1)

{

jxs_xj; //下降

while(dk8);// 下降到位检测

if(dk8==0)

{

while(dk10);//判断抓手是否有物料

jxs_zj; //抓紧

cx(200);

jxs_bc; //抓手保持

cx(100);

while(dk10)

{

jxs_fs;//抓手放松 

cx(100);

while(dk10);//再次判断抓手是否有物料

jxs_zj; //抓紧

cx(200);

jxs_bc; //抓手保持

cx(100);

}

while(dk9); //夹紧到位检测（只有当抓手把球抓紧后，程序才往下执行）

jxs_ss;//上升

while(dk7); //上升到位（如果机械手没有上升到位，程序将停在此处）

num1=0;

num2=1;

cx(255);

}

}

}

void jxs_zq1()//机械手下降抓球

{

while(num4)

{

jxs_xj; //下降

while(dk8);// 下降到位检测

if(dk8==0)

{

while(dk10);//判断抓手是否有物料

jxs_zj; //抓紧

cx(200);

jxs_bc; //抓手保持

cx(100);

while(dk10)

{

jxs_fs;//抓手放松 

cx(100);

while(dk10);//再次判断抓手是否有物料

jxs_zj; //抓紧

cx(200);

jxs_bc; //抓手保持

cx(100);

}

while(dk9); //夹紧到位检测（只有当抓手把球抓紧后，程序才往下执行）

jxs_ss;//上升

while(dk7); //上升到位（如果机械手没有上升到位，程序将停在此处）

num4=0;

num5=1;

cx(255);

}

}

} 

void init_gw1()//到到达工位1（为在工位1抓球做准备）

{

while(num)

{

if(dk4==0)//工位1检测点

{

jxs_sd;

cx(100);

jxs_sd;

num=0;

num1=1;//只有当机械手到达工位1停下之后，机械手才执行下降抓球程序

cx(200);

}

if(dk5==0)//工位2检测点

{

jxs_sd;

cx(100);

jxs_yy;//右移

cx(200);

} 

if(dk6==0)//工位3检测点

{

jxs_sd;

cx(100);

jxs_yy; //右移

cx(200);

}

}

}

void init_gw2()//在工位2抓球初始化

{

while(num3)

{

if(dk6==0)

{

jxs_yy;

cx(100); 

}

if(dk5==0)

{

jxs_sd;

cx(100);

num3=0;

num4=1;

}

}

}

void init()//系统初始化

{

jxs_sd; //电机停止

cx(255);

jxs_fs;//抓手放松

cx(255);

num=1;

if(dk6==0)//判断机械手，然后使其到达工位1

{

jxs_yy; //如果机械手在工位3，则使机械手右移 （以到达工位1）

cx(100);

}

else

{

jxs_zy;//如果机械手没在工位3，则使机械手左移

cx(100);

}

}

void main()

{

init();//系统初始化函数

while(1)

{

init_gw1(); //移动至工位1停下子函数

jxs_zq(); //机械手抓球子函数

jxs_gw3();//到工位3释放球 init_gw2();//使机械手右移（到达工位2停下）

jxs_zq1();//机械手抓球字函数

jxs_gw33(); //机械手到工位3释放球