Marcin Matras, grupa 25A, Mechatronika

Robot budowlaniec

Program manipulatora układającego piramidkę z klocków ustawionych w kilkurzędowych kolumnach postawionych obok siebie.

W programie można modyfikowac 5 zmiennych:

H- wysokość klocka

A-długośc klocka

B-szerokość klocka

X-ilość klocków postawionych na jednej kolumnie

N-ilość rzędów piramidki

10 P1 = (334.908,26.4535,79.3168,-97.0015,-179.695,0,0)

20 P2 = (97.0754,321.62,79.3168,-165.689,-179.695,0,0)

30 SERVO ON

40 H=15

50 A=20

60 B=20

70 X=5

80 N=3

90 FOR K=1 TO N

100 P1.Z=P1.Z-(K-1)*H

110 FOR I=1 TO N

120 P1.Y=P1.Y+B

130 FOR L=1 TO N

140 HOPEN 1

150 DLY 1

160 MOV P2,-50-X*H-50

170 OVRD 50

180 MOV P2,-X*H

190 HCLOSE 1

200 DLY 1

210 MOV P2,-X*H-50

220 X=X-1

230 OVRD 100

240 IF X=0 THEN P2.X=P2.X+1.5*A

250 IF X=0 THEN X=5

260 MOV P1,-50

270 OVRD 50

280 MOV P1

290 HOPEN 1

300 DLY 1

310 MOV P1,-50

320 OVRD 100

330 P1.X=P1.X+A

340 NEXT L

350 P1.X=P1.X-(N-K+1)*A+0.5*A

360 NEXT I

370 P1.Y=P1.Y-(N-K+1)*B+0.5*B

380 N=N-1

390 NEXT K

400 SERVO OFF