Projekt z labolatorium Podstaw

Robotyki

trajektoria okręgu

Tatar gr.

Rok IIIC - AiR IMIR

1. Opis zadania:

Tematem zadania jest zaprogramowanie robota tak, by po trajektorii

okręgu układał klocki warstwami.

Z powodu ograniczeń przestrzeni roboczej manipulatora nie byłem w
stanie uzyskać pełnego okręgu – skupiłem się na jego połowie.

Trajektorię okręgu uzyskałem dyskretyzując jedną z osi

współrzędnych, i obliczając drugą współrzędną z równania
opisującego trajektorię, gdzie wartością wejściową były kolejne

dyskretne wartości osi zdyskretyzowanej.

2. Kod programu:

10 OVRD 100
20 D=25 'DLUGOSC KLOCKA
30 S=25 'SZEROKOSC KLOCKA
40 W=25 'WYSOKOSC KLOCKA
50 R=275 'PROMIEN OKREGU
60 H=-50 'WYSOKOSC NAD KLOCKIEM
70 P12=P3 ‘ WYBOR KLOCKA
80 N=CINT(3.14*R/D/1.1)+1
90 O=D

100 MOV P1
110 HOPEN 1
120 P12.X=P12.X+D/2
130 P12.Y=P12.Y+S/2
140 P12.Z=P12.Z+W/4
150 P13=P3

170 M2=0

180 WHILE(M2<8)
190 M1=0
200 M3=1
210 P13.Y=R+((S/2)*M3)
220 P13.X=(D/2)*M3
230 P13.Z=P12.Z+M2*W

240 WHILE(M1<N)
250 MOV P12,H
260 MOV P12
270 DLY 0.1
280 HCLOSE 1
290 DLY 0.1
300 MOV P12,H

310 MOV P13,H
320 MOV P13
330 DLY 0.1
340 HOPEN 1
350 DLY 0.1
360 MOV P13,H

370 Y13=P13.Y-D
380 X13=ABS(SQR(R*R-Y13*Y13))
390 B=ABS(X13-P13.X)
400 IF B>O THEN *L1 ELSE *L4
410 *L1
420 IF Y13>=0 THEN *L2 ELSE *L3
430 *L2
440 X13=P13.X+S
450 Y13=ABS(SQR(R*R-X13*X13))
460 GOTO *L4
470 *L3
480 X13=P13.X-S
490 Y13=-ABS(SQR(R*R-X13*X13))
500 GOTO *L4

510 *L4
520 P13.X=X13
530 P13.Y=Y13
540 M1=M1+1
550 WEND
560 M2=M2+1
570 M3=M3-1
580 WEND

3. Efekty działania programu:

rys.1. półokrąg ułożony z mniejszych klocków o mniejszej średnicy

rys.1. półokrąg ułożony z większych klocków o większej średnicy + śledzenie ruchów