-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathMaster.bs1
187 lines (185 loc) · 3.58 KB
/
Master.bs1
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
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
' {$STAMP BS1}
'W0 = UltraRechts Ergebnis
'W1 = UltraVorne Ergebnis
'W2 = ServoPause
'W3 = LEDLinks
'W4 = LEDRechts
'W5 = Brems
'W6 = LEDAlle
SYMBOL Ultra = 7
SYMBOL UltraVorne = 6
SYMBOL UltraRechts = 5
SYMBOL LichtSchranke = 4
SYMBOL ServoMotor = 3
SYMBOL ElektroMotor1 = 2
SYMBOL ElektroMotor2 = 1
SYMBOL Signal = 0
SYMBOL ServoPause = W2
OUTPUT Ultra
OUTPUT Signal
OUTPUT ServoMotor
OUTPUT ElektroMotor1
OUTPUT ElektroMotor2
INPUT UltraVorne
INPUT UltraRechts
INPUT LichtSchranke
INPUT Signal
W2 = 200
W3 = 0
W4 = 0
W5 = 0
W6 = 0
GOTO StartLoop
END
StartLoop:
PULSOUT ServoMotor, 150
PAUSE 100
LOW ServoMotor
LOW Ultra
PULSOUT Ultra, 1
PULSIN UltraVorne, 1, W1
W1 = W1 / 6
IF W1 < 25 THEN DriveLoop
GOTO StartLoop
DriveLoop:
LOW Ultra
PULSOUT Ultra, 1
PULSIN UltraVorne, 1, W1
W1 = W1 / 6
IF W1 < 15 THEN TurnLoop
LOW ElektroMotor1
HIGH ElektroMotor2
PAUSE 200
GOTO DriveLoop
TurnLoop:
LOW ElektroMotor1
LOW ElektroMotor2
SEROUT Signal, T2400, (5)
PULSOUT ServoMotor, 180
SEROUT Signal, T2400, (5)
PAUSE 2500
SEROUT Signal, T2400, (5)
LOW ServoMotor
SEROUT Signal, T2400, (2)
LOW ElektroMotor1
HIGH ElektroMotor2
SEROUT Signal, T2400, (2)
PAUSE 6500
SEROUT Signal, T2400, (5)
LOW ElektroMotor1
LOW ElektroMotor2
SEROUT Signal, T2400, (5)
GOTO ParkSearchLoopStart
ParkSearchLoopStart:
SEROUT Signal, T2400, (1)
SEROUT Signal, T2400, (1)
LOW ElektroMotor1
HIGH ElektroMotor2
GOTO ParkSearchLoop
ParkSearchLoop:
SEROUT Signal, T2400, (1)
LOW Ultra
PULSOUT Ultra, 1
PULSIN UltraRechts, 1, W0
W0 = W0 / 6
IF W0 > 30 THEN ParkFoundLoop
PULSOUT ServoMotor, 140
PAUSE 75
LOW ServoMotor
IF PIN4=1 THEN AusgleichenSearch
ParkSearchCont:
SEROUT Signal, T2400, (1)
GOTO ParkSearchLoop
AusgleichenSearch:
PULSOUT ServoMotor, 170
PAUSE 120
LOW ServoMotor
GOTO ParkSearchCont
ParkFoundLoop:
SEROUT Signal, T2400, (1)
LOW Ultra
PULSOUT Ultra, 1
PULSIN UltraRechts, 1, W0
W0 = W0 / 6
IF W0 < 30 THEN ParkEndLoop
PULSOUT ServoMotor, 140
PAUSE 75
LOW ServoMotor
IF PIN4=1 THEN AusgleichenFound
ParkFoundCont:
SEROUT Signal, T2400, (1)
GOTO ParkFoundLoop
AusgleichenFound:
PULSOUT ServoMotor, 170
PAUSE 120
LOW ServoMotor
GOTO ParkFoundCont
ParkEndLoop:
SEROUT Signal, T2400, (5)
LOW ElektroMotor1
low ElektroMotor2
SEROUT Signal, T2400, (5)
PULSOUT ServoMotor, 120
SEROUT Signal, T2400, (5)
PAUSE 3000
SEROUT Signal, T2400, (3)
LOW ServoMotor
SEROUT Signal, T2400, (3)
HIGH ElektroMotor1
LOW ElektroMotor2
PAUSE 3000
SEROUT Signal, T2400, (5)
low ElektroMotor1
LOW ElektroMotor2
SEROUT Signal, T2400, (5)
PULSOUT ServoMotor, 180
PAUSE 3000
SEROUT Signal, T2400, (2)
LOW ServoMotor
SEROUT Signal, T2400, (2)
HIGH ElektroMotor1
LOW ElektroMotor2
SEROUT Signal, T2400, (2)
PAUSE 3000
low ElektroMotor1
LOW ElektroMotor2
SEROUT Signal, T2400, (1)
SEROUT Signal, T2400, (1)
SEROUT Signal, T2400, (1)
END
ServoLinks:
PULSOUT ServoMotor, 180
PAUSE ServoPause
LOW ServoMotor
ServoRechts:
PULSOUT ServoMotor, 120
PAUSE ServoPause
LOW ServoMotor
ServoMinimalRechts:
PULSOUT ServoMotor, 120
PAUSE 50
LOW ServoMotor
ServoMinimalLinks:
PULSOUT ServoMotor, 180
PAUSE 50
LOW ServoMotor
VorwaertsFahren:
LOW ElektroMotor1
HIGH ElektroMotor2
RuekwaertsFahren:
HIGH ElektroMotor1
LOW ElektroMotor2
UltraRechtsMessen:
LOW Ultra
PULSOUT Ultra, 1
PULSIN UltraRechts, 1, W0
W0 = W0 / 6
UltraVorneMessen:
LOW Ultra
PULSOUT Ultra, 1
PULSIN UltraVorne, 1, W1
W1 = W1 / 6
SendLEDInfo:
W6 = W3 + W4*2 + W5*4 + 1
SEROUT Signal, T2400, (W6)
END