move.lun

scara机器人的一种简单的实现方法

INT:     I, GD, GD1, B1, B2, B3, FUHAO, RSERR, N
REAL:    DAT
POINT:   PT1, PT2

	 FORM(RIGHT)
10       CALL    200S
	 GO      10


;        ******************************
;        receive 1byte & send back
;        ******************************
120S     I = 0
	 RSERR = 0
120      RSINP(GD)
	 IF GD = 0  GO 121
	 RSOUT(GD)
	 GD1 = GD
	 I = I + 1
	 IF I<5  GO 120
	 GO  122
121      IF I=0  GO 120
	 GO  123
122      RSERR = 1
123      RET


;        ******************************
;        get dest. position data
;        ******************************
150S     FUHAO = 1
	 CALL    120S
	 IF RSERR=1 STOP
	 B1 = GD1
	 CALL    120S
	 IF RSERR=1 STOP
	 B2 = GD1
	 CALL    120S
	 IF RSERR=1 STOP
	 B3 = GD1
	 IF B1<128 GO 150
	 B1 = B1 - 128
	 FUHAO = -1
150      IF B1=64  GO 165
	 IF B1<64  GO 160
	 B1 = B1 - 128
160      DAT = FUHAO*(B2/256+B3/256/256)*(2^B1)
	 GO 170
165      DAT = 0.0
170      RET

;        ******************************
;	 move to dest. position
;        ******************************
200S     PT1 = PT2
         CALL    150S
         PT2(X) = DAT
	 CALL    150S
         PT2(Y) = DAT
	 CALL    150S
         PT2(Z) = DAT
	 CALL    150S
         PT2(R) = DAT
         N = 20
         IF ABS(PT2(X)-PT1(X))+ABS(PT2(Y)-PT1(Y))<50 THEN N = 80
         IF ABS(PT2(Z)-PT1(Z))>0.9 THEN N = 30
	 IF ABS(PT2(R)-PT1(R))>100 THEN N = 30
	 DO  VEL(N) PT2
	 RET

	 END