YAMAHA机器人视觉程序

　　[PGM]

　　NAME=SHIFT

　　PGN=1

　　'===================声明数组============

　　DIM PPS$(5)

　　DIM PPX$(5)

　　DIM PPY$(5)

　　DIM PPR$(5)

　　DIM PPX!(5)

　　DIM PPY!(5)

　　DIM PPR!(5)

　　A%=1

　　'=======================================

　　*A:

　　DO2()=0

　　MOVE P,P0,Z=0.00

　　WAIT ARM

　　'======================================

　　MOVE P,P101,Z=0.00 '1拍照位置

　　WAIT ARM

　　'======================================

　　*PAIZHAO1:

　　SEND "AA" TO GP1

　　SEND GP1 TO DATA$

　　PRINT "DATA$=",DATA$

　　PPS$(1)=MID$(DATA$,1,2)

　　IF PPS$(1)="OK" THEN

　　GOTO *PAIZHAOOK1

　　ENDIF

　　IF PPS$(1)="NG" THEN

　　DELAY 100

　　GOTO *PAIZHAO1

　　ENDIF

　　'======================================

　　*PAIZHAOOK1:

　　PPX$(1)=MID$(DATA$,3,8)

　　PPY$(1)=MID$(DATA$,11,8)

　　PPR$(1)=MID$(DATA$,19,8)

　　PPX!(1)=VAL(PPX$(1))

　　PPY!(1)=VAL(PPX$(1))

　　PPR!(1)=VAL(PPX$(1))

　　LOC1(P11)=PPX!(1)

　　LOC2(P11)=PPY!(1)

　　LOC4(P11)=PPR!(1)

　　LOC3(P11)=20.00

　　'========================================

　　MOVE P,P102,Z=0.00 '2拍照位置

　　WAIT ARM

　　'======================================

　　*PAIZHAO2:

　　SEND "AB" TO GP1

　　SEND GP1 TO DATA$

　　PPS$(2)=MID$(DATA$,1,2)

　　PRINT "DATA$=",DATA$

　　IF PPS$(2)="OK" THEN

　　GOTO *PAIZHAOOK2

　　ENDIF

　　IF PPS$(2)="NG" THEN

　　DELAY 100

　　GOTO *PAIZHAO2

　　ENDIF

　　'=====================================

　　*PAIZHAOOK2:

　　PPX$(2)=MID$(DATA$,3,8)

　　PPY$(2)=MID$(DATA$,11,8)

　　PPR$(2)=MID$(DATA$,19,8)

　　PPX!(2)=VAL(PPX$(2))

　　PPY!(2)=VAL(PPX$(2))

　　PPR!(2)=VAL(PPX$(2))

　　LOC1(P12)=PPX!(2)

　　LOC2(P12)=PPY!(2)

　　LOC4(P12)=PPR!(2)

　　LOC3(P12)=20.00

　　'=========================================

　　'=============偏移公式计算偏移S1============

　　DLTX1=LOC1(P2)-LOC1(P1)

　　DLTY1=LOC2(P2)-LOC2(P1)

　　ANG1=ATN(DLTY1/DLTX1)

　　DLTX11=LOC1(P12)-LOC1(P11)

　　DLTY11=LOC2(P12)-LOC2(P11)

　　ANG11=ATN(DLTY11/DLTX11)

　　THETA=ANG11-ANG1

　　LOC1(P1001)=LOC1(P1)*COS(THETA)-LOC2(P1)*SIN(THETA)

　　LOC2(P1001)=LOC1(P1)*SIN(THETA)+LOC2(P1)*COS(THETA)

　　DSTX1=LOC1(P11)-LOC1(P1001)

　　DSTY1=LOC2(P11)-LOC2(P1001)

　　LOC1(P1002)=LOC1(P2)*COS(THETA)-LOC2(P2)*SIN(THETA)

　　LOC2(P1002)=LOC1(P2)*SIN(THETA)+LOC2(P2)*COS(THETA)

　　DSTX2=LOC1(P12)-LOC1(P1002)

　　DSTY2=LOC2(P12)-LOC2(P1002)

　　DSTX=(DSTX1+DSTX2)/2

　　DSTY=(DSTY1+DSTY2)/2

　　LOC1(S1)=LOC1(S0)+DSTX

　　LOC2(S1)=LOC2(S0)+DSTY

　　LOC4(S1)=LOC4(S0)+RADDEG(THETA)

　　LOC3(S1)=20.00

　　'================================================

　　'================================================

　　*MAIN:

　　FOR B%=1 TO 32

　　C%=B%+499

　　P[C%]=P0

　　NEXT

　　'======================================

　　FOR B%=1 TO 32

　　C%=B%+499

　　P[C%]=PPNT(0,B%)

　　NEXT

　　'=======================================

　　MOVE P,P103,Z=0.00    '3拍照位置

　　WAIT ARM

　　'=======================================

　　*PAIZHAO3:

　　SEND "AC" TO GP1

　　SEND GP1 TO DATA$

　　PPS$(3)=MID$(DATA$,1,2)

　　PRINT "DATA$=",DATA$

　　IF PPS$(3)="OK" THEN

　　GOTO *PAIZHAOOK3

　　ENDIF

　　IF PPS$(3)="NG" THEN

　　DELAY 100

　　GOTO *PAIZHAO3

　　ENDIF

　　'=======================================

　　*PAIZHAOOK3:

　　PPX$(3)=MID$(DATA$,3,8)

　　PPY$(3)=MID$(DATA$,11,8)

　　PPR$(3)=MID$(DATA$,19,8)

　　PPX!(3)=VAL(PPX$(3))

　　PPY!(3)=VAL(PPX$(3))

　　PPR!(3)=VAL(PPX$(3))

　　LOC1(P13)=PPX!(3)

　　LOC2(P13)=PPY!(3)

　　LOC4(P13)=PPR!(3)

　　LOC3(P13)=20.00

　　SHIFT S0

　　MOVE P,P13,Z=0.00    '走到吸料位

　　WAIT ARM

　　DO(23)=1             '吸料

　　DELAY 100

　　'=========================================

　　MOVE P,P104,Z=0.00    '4拍照位置，矫正

　　WAIT ARM

　　'=======================================

　　*PAIZHAO4:

　　SEND "AD" TO GP1

　　SEND GP1 TO DATA$

　　PPS$(4)=MID$(DATA$,1,2)

　　PRINT "DATA$=",DATA$

　　IF PPS$(4)="OK" THEN

　　GOTO *PAIZHAOOK4

　　ENDIF

　　IF PPS$(4)="NG" THEN

　　DELAY 100

　　GOTO *PAIZHAO4

　　ENDIF

　　'=====================================

　　*PAIZHAOOK4:          '二次矫正拍照,固定相机

　　PPX$(4)=MID$(DATA$,3,8)

　　PPY$(4)=MID$(DATA$,11,8)

　　PPR$(4)=MID$(DATA$,19,8)

　　PPX!(4)=VAL(PPX$(4))

　　PPY!(4)=VAL(PPX$(4))

　　PPR!(4)=VAL(PPX$(4))

　　LOC1(P14)=PPX!(4)

　　LOC2(P14)=PPY!(4)

　　LOC4(P14)=PPR!(4)

　　LOC3(P14)=20.00

　　SHIFT S0

　　MOVE P,P14,Z=0.00             '矫正角度

　　WAIT ARM

　　'=====================================

　　SHIFT S1

　　MOVE P,P[A%+499],Z=0.00

　　WAIT ARM

　　DO(23)=0

　　'====================================

　　SHIFT S0

　　MOVE P,P105,Z=0.00    '5拍照位置

　　WAIT ARM

　　'===================================

　　*PAIZHAOOK5:

　　SEND "AE" TO GP1

　　SEND GP1 TO DATA$

　　PPS$(5)=MID$(DATA$,1,2)

　　PRINT "DATA$=",DATA$

　　IF PPS$(5)="OK" THEN

　　GOTO *PAIZHAOOK5

　　ENDIF

　　IF PPS$(5)="NG" THEN

　　DELAY 100

　　GOTO *PAIZHAO6

　　ENDIF

　　'==================================

　　*PAIZHAO6:

　　PPX$(5)=MID$(DATA$,3,8)

　　PPY$(5)=MID$(DATA$,11,8)

　　PPR$(5)=MID$(DATA$,19,8)

　　PPX!(5)=VAL(PPX$(5))

　　PPY!(5)=VAL(PPX$(5))

　　PPR!(5)=VAL(PPX$(5))

　　LOC1(P15)=PPX!(5)

　　LOC2(P15)=PPY!(5)

　　LOC4(P15)=PPR!(5)

　　LOC3(P15)=20.00

　　SHIFT S0

　　MOVE P,P15,Z=0.00    '走到吸料位

　　WAIT ARM

　　DO(23)=1             '吸料

　　DELAY 100

　　'======================================

　　MOVE P,P104,Z=0.00    '4拍照位置，矫正，固定相机

　　WAIT ARM

　　'=======================================

　　*PAIZHAO4:

　　SEND "AD" TO GP1

　　SEND GP1 TO DATA$

　　PPS$(4)=MID$(DATA$,1,2)

　　PRINT "DATA$=",DATA$

　　IF PPS$(4)="OK" THEN

　　GOTO *PAIZHAOOK4

　　ENDIF

　　IF PPS$(4)="NG" THEN

　　DELAY 100

　　GOTO *PAIZHAO4

　　ENDIF

　　'=====================================

　　*PAIZHAOOK4:          '二次矫正拍照

　　PPX$(4)=MID$(DATA$,3,8)

　　PPY$(4)=MID$(DATA$,11,8)

　　PPR$(4)=MID$(DATA$,19,8)

　　PPX!(4)=VAL(PPX$(4))

　　PPY!(4)=VAL(PPX$(4))

　　PPR!(4)=VAL(PPX$(4))

　　LOC1(P14)=PPX!(4)

　　LOC2(P14)=PPY!(4)

　　LOC4(P14)=PPR!(4)

　　LOC3(P14)=20.00

　　SHIFT S0

　　MOVE P,P14,Z=0.00    '矫正角度

　　WAIT ARM

　　A%=A%+1

　　SHIFT S1

　　MOVE P,P[A%+499],Z=0.00

　　WAIT ARM

　　DO(23)=0

　　A%=A%+1

　　IF A%=33 THEN

　　A%=1

　　GOTO *A

　　ENDIF

　　GOTO *MAIN

　　[GEP]

　　GP1

　　MODE=1

　　IPADRS=192.168.0.5

　　PORT=5

　　EOL=0

　　TYPE=0

　　[END]

YAMAHA机器人视觉程序

联系方式

工作时间