B3RoboticsTutorial/Task2.prg at main · alantorresve/B3RoboticsTutorial · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
Integer TRAY
Integer ITEM_TYPE
Integer ITEM_POS
Integer HEIGHT
'Points needed : PT1, PT2, TF, BF, START
''ITEM_TYPE = 0 //Token
''ITEM_TYPE = 1 //Block

Function Task2

Motor On
Power High
Speed 30
Accel 30, 30
SpeedS 500
AccelS 5000
Tool 1

Go START

HEIGHT = 0
For TRAY = 1 To 2

              For ITEM_POS = 0 To 2
                            For ITEM_TYPE = 0 To 1
                                          PickPlace()
                            Next ITEM_TYPE
              Next ITEM_POS

Next
Go START
Fend

Function PickPlace

              If TRAY = 1 Then

                            Go PT1 +Z(15) +Z(HEIGHT * 6) +Y(ITEM_POS * 30.7) +X(ITEM_TYPE * 30) /2
                            Go PT1 +Y(ITEM_POS * 30.7) +X(ITEM_TYPE * 30) +X(-ITEM_POS / 2) /2
                            On 8
                            Wait 0.5
                            Go PT1 +Z(15) +Z(HEIGHT * 6) +Y(ITEM_POS * 30.7) +X(ITEM_TYPE * 30) /2

                            Go BA +Z(HEIGHT * 6) +Z(15) /2 ' Secure Block Feeder
                            Go BA +Z(HEIGHT * 6) /2 ' Block Feeder
                            Off 8
                            Wait 0.5
                            Go BA +Z(HEIGHT * 6) +Z(15) /2 ' Secure Block Feeder
                            HEIGHT = HEIGHT + 1


              Else

                            Go PT2 +Z(15) +Z(HEIGHT * 6) +Y(ITEM_POS * 30.7) +X(ITEM_TYPE * 30) /2
                            Go PT2 +Y(ITEM_POS * 30) +X(ITEM_TYPE * 30) +Z(-ITEM_POS) +X(-ITEM_POS / 2) /2
                            On 8
                            Wait 0.5
                            Go PT2 +Z(15) +Z(HEIGHT * 6) +Y(ITEM_POS * 30.7) +X(ITEM_TYPE * 30) /2

                            Go BA +Z(HEIGHT * 6) +Z(15) /2 ' Secure Block Feeder
                            Go BA +Z(HEIGHT * 6) /2 ' Block Feeder
                            Off 8
                            Wait 0.5
                            Go BA +Z(HEIGHT * 6) +Z(15) /2 ' Secure Block Feeder
                            HEIGHT = HEIGHT + 1
              EndIf

Fend