forked from Igzi/ABB-Robo-Challange-2021
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathModuleR1.mod
151 lines (144 loc) · 5.72 KB
/
ModuleR1.mod
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
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
MODULE Module1
CONST robtarget PocetnaPozicija0:=[[0,-50,0],[1,0,0,0],[1,-1,2,4],[-101.964427132,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget DozerL:=[[0,-50,0],[1,0,0,0],[1,0,1,4],[-101.96443337,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget DozerL_Podignuta:=[[0,-61,0],[1,0,0,0],[1,0,1,4],[-101.96443337,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget DozerP:=[[0,-50,0],[1,0,0,0],[1,1,0,4],[-101.964429518,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget DozerP_Podignuta:=[[0,-61,0],[1,0,0,0],[1,1,0,4],[-101.964429518,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget MedjuPozicija:=[[0,-50,0],[1,0,0,0],[1,0,1,4],[-101.964433413,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget IznadCase:=[[260.103,-37.879,124.7],[0.707106781,-0.707106781,0,0],[1,1,1,4],[121.964437875,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget NakrenutaFlasa:=[[260.103,-37.879,124.7],[0.130526192,-0.991444861,0,0],[1,1,1,4],[121.964437875,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS tasks task_list{2}:=[["T_ROB_L"],["T_ROB_R"]];
PERS num count1 := 0;
PERS num count2 := 0;
PERS num count3 := 0;
PERS num count4 := 0;
PERS num count5 := 0;
PERS num bExit1 := 1;
VAR num tmp1 := 0;
VAR syncident cep_skinut;
VAR syncident cep_sipano;
VAR syncident zavrnuto;
VAR syncident desna_uzela_flasu;
VAR syncident desna_pustila_flasu;
VAR syncident sok;
VAR syncident ucitane_vrednosti;
VAR syncident ucitan_izlaz;
VAR syncident doneta_flasica;
VAR syncident ucitana_vrednost;
PROC main()
VAR num bExit2:=1;
TPReadFK bExit1, "Pritisnite dugme Zapocni ukoliko zelite da napravite koktel a Zavrsi u suprotnom", "Zapocni", stEmpty, stEmpty, stEmpty, "Zavrsi";
WHILE bExit1=1 DO
count1 := 0;
count2 := 0;
count3 := 0;
count4 := 0;
count5 := 0;
bExit2 := 1;
WHILE bExit2=1 DO
TPReadFK tmp1, "Odaberite sastojak koji zelite da ubacite u koktel", "Pomorandza", "Limun", "Fanta", "Sprite", "CocaCola";
IF tmp1 = 1 THEN
count1:=count1+1;
ELSEIF tmp1 = 2 THEN
count2:=count2+1;
ELSEIF tmp1 = 3 THEN
count3:=count3+1;
ELSEIF tmp1 = 4 THEN
count4:=count4+1;
ELSEIF tmp1 = 5 THEN
count5:=count5+1;
ENDIF
TPReadFK bExit2, "Pritisnite dugme Dodaj za novi sastojak ili Zavrsi u suprotnom", "Dodaj", stEmpty, stEmpty, stEmpty, "Zavrsi";
ENDWHILE
IF count3 > 1 OR count4 > 1 OR count5 > 1 THEN
WaitSyncTask ucitana_vrednost,task_list;
TPWrite("Nemoguce je napraviti koktel sa unetim vrednostima");
ELSE
WaitSyncTask ucitana_vrednost,task_list;
IF count3 > 0 THEN
SipajGazirano;
ENDIF
IF count4 > 0 THEN
SipajGazirano;
ENDIF
IF count5 > 0 THEN
SipajGazirano;
ENDIF
IF count1>0 OR count2>0 THEN
PokupiCasu;
ENDIF
IF count1>0 THEN
UradiMedjuKorak;
ENDIF
WHILE count1>0 DO
SipajPomorandzu;
count1 := count1-1;
ENDWHILE
IF count2>0 THEN
UradiMedjuKorak;
ENDIF
WHILE count2>0 DO
SipajLimunadu;
count2 := count2-1;
ENDWHILE
OstaviCasu;
ENDIF
TPReadFK bExit1, "Pritisnite dugme Zapocni za novi koktel ili Zavrsi u suprotnom", "Zapocni", stEmpty, stEmpty, stEmpty, "Zavrsi";
WaitSyncTask ucitan_izlaz,task_list;
ENDWHILE
ENDPROC
PROC SipajGazirano()
DodjiDoFlase;
WaitSyncTask \InPos,doneta_flasica,task_list;
Zgrabi;
WaitSyncTask \InPos,desna_uzela_flasu,task_list;
WaitSyncTask \InPos,cep_skinut,task_list;
Sipaj;
OkreniNatrag;
WaitSyncTask \InPos,cep_sipano,task_list;
WaitSyncTask \InPos,zavrnuto,task_list;
Pusti;
WaitSyncTask \InPos,desna_pustila_flasu,task_list;
ENDPROC
PROC SipajLimunadu()
MoveL DozerL,v1000,z100,Servo\WObj:=CasaLimunada;
MoveL DozerL_Podignuta,v1000,z100,Servo\WObj:=CasaLimunada;
WaitTime \InPos,2;
MoveL DozerL,v1000,z100,Servo\WObj:=CasaLimunada;
ENDPROC
PROC PokupiCasu()
MoveL PocetnaPozicija0,v1000,z100,Servo\WObj:=PocetakCasa;
Zgrabi;
ENDPROC
PROC OstaviCasu()
MoveL PocetnaPozicija0,v1000,z100,Servo\WObj:=PocetakCasa;
Pusti;
ENDPROC
PROC SipajPomorandzu()
MoveL DozerP,v1000,z100,Servo\WObj:=CasaPomorandza;
MoveL DozerP_Podignuta,v1000,z100,Servo\WObj:=CasaPomorandza;
WaitTime \InPos,2;
MoveL DozerP,v1000,z100,Servo\WObj:=CasaPomorandza;
ENDPROC
PROC UradiMedjuKorak()
MoveL MedjuPozicija,v1000,z100,Servo\WObj:=MedjuKorak;
ENDPROC
PROC DodjiDoFlase()
MoveL IznadCase,v1000,z100,Servo\WObj:=FlasicaR1;
ENDPROC
PROC Sipaj()
MoveL NakrenutaFlasa,v1000,z100,Servo\WObj:=FlasicaR1;
ENDPROC
PROC OkreniNatrag()
MoveL NakrenutaFlasa,v1000,z100,Servo\WObj:=FlasicaR1;
MoveL IznadCase,v1000,z100,Servo\WObj:=FlasicaR1;
ENDPROC
PROC Zgrabi()
g_GripIn;
WaitTime\InPos,0.5;
ENDPROC
PROC Pusti()
g_JogOut;
WaitTime\InPos,0.5;
ENDPROC
ENDMODULE