川崎机器人怎样操作返回原点_川崎机器人程序

pica[ca] BY 100,0,150)

VALUE 4:

POINT pica[ca] = SHIFT(pica1 BY 645,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 100,0,150)

VALUE 5:

POINT pica[ca] = SHIFT(pica1 BY 645,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 100,0,150)

VALUE 6:

POINT pica[ca] = SHIFT(pica1 BY 645,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 100,0,150)

END

.END

.PROGRAM pointsoa() #0

;位置a偶数层位置计算

POINT pica2 = SHIFT(picao_t BY 0,0,0)

CASE d OF

VALUE 0:

POINT pica[ca] = SHIFT(pica2 BY 0,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 80,0,150)

VALUE 1:

POINT pica[ca] = SHIFT(pica2 BY 343.5,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 100,0,150)

VALUE 2:

POINT pica[ca] = SHIFT(pica2 BY 645,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 100,0,150)

VALUE 3:

POINT pica[ca] = SHIFT(pica2 BY 645,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 100,0,150)

VALUE 4:

POINT pica[ca] = SHIFT(pica2 BY 645,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 100,0,150)

VALUE 5:

POINT pica[ca] = SHIFT(pica2 BY 645,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 100,0,150)

VALUE 6:

POINT pica[ca] = SHIFT(pica2 BY 645,0,z)

POINT picaw[ca] = SHIFT(pica[ca] BY 100,0,150)

END

.END

.PROGRAM dousa() #0

;动作主程序

SPEED 100 ALWAYS

ACCURACY 500 ALWAYS

100

ONI 1004 CALL stop;监视停止信号，并调用停止程序

IF SIG(1021)==TRUE THEN

CALL dousal;监视料斗状态，并调用动作程序

END

IF SIG(1022)==TRUE THEN

CALL dousal

END

IF SIG(1023)==TRUE THEN

CALL dousar

END

IF SIG(1024)==TRUE THEN

CALL dousar

END

GOTO 100

.END

.PROGRAM stop() #0

;停止程序

SPEED 50 ALWAYS

ACCURACY 50 ALWAYS

HERE tt

POINT/Z tt = gen

LMOVE tt;移动到安全位置

LMOVE ho;返回原点

DO

PAUSE

SIGNA

UNTIL SIG(1002)

.END

.PROGRAM dousal() #0

;左侧动作程序

50

IF SIG(1013,1021)==TRUE THEN

CALL dousaa1;判断托盘和料斗信号

IF ca&lt41 THEN

ca=ca+1

ELSE

ca=0

END

GOTO 50

END

IF SIG(1013,1022)==TRUE THEN

CALL dousaa2

IF ca&lt41 THEN

ca=ca+1

ELSE

ca=0

END

GOTO 50

END

60

IF SIG(1014,1021)==TRUE THEN

CALL dousab1

IF cb&lt41 THEN

cb=cb+1

ELSE

cb=0

END

GOTO 60

END

IF SIG(1014,1022)==TRUE THEN

CALL dousab2

IF cb&lt41 THEN

cb=cb+1

ELSE

cb=0

END

GOTO 60

END

RETURN

.END

.PROGRAM dousar() #0

;右侧动作程序

70

IF SIG(1015,1023)==TRUE THEN

CALL dousac3;判断托盘和料斗信号

IF cc&lt41 THEN

cc=cc+1

ELSE

cc=0

END

GOTO 70

END

IF SIG(1015,1024)==TRUE THEN

CALL dousac4

IF cc&lt41 THEN

cc=cc+1

ELSE

cc=0

END

GOTO 70

END

80

IF SIG(1016,1023)==TRUE THEN

CALL dousad3

IF cd&lt41 THEN

cd=cd+1

ELSE

cd=0

END

GOTO 80

END

IF SIG(1016,1024)==TRUE THEN

CALL dousad4

IF cd&lt41 THEN

cb=cd+1

ELSE

cd=0

END

GOTO 80

END

RETURN

.END

.PROGRAM dousaa1() #0

;从A托盘取料放到1号料斗

SPEED 100 ALWAYS

ACCURACY 500 ALWAYS

TIMER 1=0;计时器清零

200

LMOVE pupo0;移