1 '(
2 r o v e r
3 ššššššššš
4 test code for rover based on 8535
5 ')
6
7 M1A alias portd.7
8 M1B alias portd.6
9
10 M2A alias portc.1
11 M2B alias portc.0
12
13 BAT_CONTROL alias portc.4
14 const BAT_LEVEL = 5
15
16 TOUCH_BACK_LEFT alias pinc.5
17 TOUCH_BACK_RIGHT alias pinc.6
18 TOUCH_FRONT_LEFT alias pind.4
19 TOUCH_FRONT_RIGHT alias pind.5
20
21 EYE_CONTROL alias portc.7
22 const EYE_LEFT = 7
23 const EYE_RIGHT = 6
24
25 config Adc = Single , Prescaler = Auto
26 config debounce = 10
27 config Rc5 = pind.2
28 enable interrupts
29
30 ' -- subroutine declarations
31 declare sub forward
32 declare sub backward
33 declare sub turn_left
34 declare sub turn_right
35 declare sub halt
36 declare sub diagnostics
37
38 declare sub motor
39
40 declare sub touch
41 declare sub photofoob
42 declare sub wander
43
44 ' -- variable declarations
45 dim eye_l as Word
46 dim eye_r as Word
47 dim bat as Word
48 dim tch_bl as bit
49 dim tch_br as bit
50 dim tch_fl as bit
51 dim tch_fr as bit
52
53 dim c as byte
54 dim rc5_address as byte, rc5_command as byte
55 dim temp as byte
56 dim i as integer
57
58 ' -- define behaviours
59
60 ' . global settings
61 const BEAT = 50 ' -- unit of execution time
62 ' a behaviour will use this as a standard
63 ' time tick for action
64 const BEAT2 = 100
65
66 ' . touch
67 dim touch_state as byte ' - state of the 'touch' behaviour
68 dim touch_dur as word ' - duration
69 dim touch_turn as byte
70 dim touch_direction as byte
71
72 ' . motor
73 dim motor_vector as byte ' - drive vector for motors
74 const M_STOP = 0
75 const M_FORWARD = 1
76 const M_RIGHT = 2
77 const M_BACK = 3
78 const M_LEFT = 4
79
80 ' . photofoob
81 dim photofoob_state as byte
82 dim photofoob_turn as byte
83 dim photofoob_dur as word
84 dim photofoob_offset as word
85
86 ' . wander
87 dim wander_state as byte
88 dim wander_turnorgo as bit
89 dim wander_dur as long
90 const WANDER_RUN = 200*BEAT
91
92 ' -- main
93 DDRD = &B11001011
94 DDRC = &B10011111
95
96 start Adc
97
98 set PORTC.6
99 set PORTC.5
100 set PORTD.5
101 set PORTD.4
102
103 wander_state = 0
104 photofoob_state = 0
105 photofoob_offset = 50
106 touch_state = 0
107 motor_vector = 0
108
109 do
110 ' pull in data from the sensors
111 gosub latch_sensors
112
113 ' behaviours
114 photofoob
115 wander
116 touch
117
118 ' override controls now with RC5 commands
119 ' gosub remote
120
121 ' motor control
122 motor
123
124 temp = inkey()
125 if temp > 0 then
126 call diagnostics
127 end if
128
129 loop
130
131 ' -- control the engine
132 sub motor
133 select case motor_vector
134 case M_STOP:
135 call halt
136 case M_FORWARD:
137 call forward
138 case M_BACK:
139 call backward
140 case M_LEFT:
141 call turn_left
142 case M_RIGHT:
143 call turn_right
144 end select
145 end sub
146
147 ' -- Pull the sensory data into globals
148 latch_sensors:
149 set EYE_CONTROL
150 waitus 250
151 c = EYE_LEFT
152 eye_l = getadc(c)
153 c = EYE_RIGHT
154 eye_r = getadc(c)
155 reset EYE_CONTROL
156
157 set BAT_CONTROL
158 waitus 250
159 c = BAT_LEVEL
160 bat = getadc(c)
161 reset BAT_CONTROL
162
163 tch_bl = TOUCH_BACK_LEFT
164 tch_br = TOUCH_BACK_RIGHT
165 tch_fl = TOUCH_FRONT_LEFT
166 tch_fr = TOUCH_FRONT_RIGHT
167
168 return
169
170 ' --
171 remote:
172 getrc5( rc5_address, rc5_command)
173 if rc5_address <> 255 then
174 rc5_command = rc5_command and &B10111111
175 select case rc5_command
176 case 32:
177 motor_vector = M_FORWARD
178 case 33:
179 motor_vector = M_BACK
180 case 16:
181 motor_vector = M_RIGHT
182 case 17:
183 motor_vector = M_LEFT
184 case 12:
185 motor_vector = M_STOP
186 case 14:
187 call diagnostics
188 end select
189 end if
190 return
191
192 ' -- print out some diagnostics
193 sub diagnostics
194 print "el "; eye_l
195 print "er "; eye_r
196 print "ba "; bat
197 print "wd "; wander_dur
198 print "bl "; tch_bl
199 print "br "; tch_br
200 print "fl "; tch_fl
201 print "fr "; tch_fr
202 print "vc "; motor_vector
203 end sub
204
205 ' -- move forward
206 sub forward
207 set M1A
208 reset M1B
209 set M2A
210 reset M2B
211 end sub
212
213 ' -- move backward
214 sub backward
215 reset M1A
216 set M1B
217 reset M2A
218 set M2B
219 end sub
220
221 ' -- turn right
222 sub turn_right
223 set M1A
224 reset M1B
225 reset M2A
226 set M2B
227 end sub
228
229 ' -- turn left
230 sub turn_left
231 reset M1A
232 set M1B
233 set M2A
234 reset M2B
235 end sub
236
237 ' -- stop rover
238 sub halt
239 reset M1A
240 reset M1B
241 reset M2A
242 reset M2B
243 end sub
244
245
246 ' ------------------------------------------------------------------------
247 ' -- B E H A V I O U R S
248 ' ------------------------------------------------------------------------
249
250 sub touch
251 ' we always react to touch, even when processing a touch
252 if tch_bl = 0 then
253 touch_turn = M_RIGHT
254 touch_direction = M_FORWARD
255 touch_state = 1
256 touch_dur = BEAT2
257 end if
258
259 if tch_br = 0 then
260 touch_turn = M_LEFT
261 touch_direction = M_FORWARD
262 touch_state = 1
263 touch_dur = BEAT2
264 end if
265
266 if tch_fl = 0 then
267 touch_turn = M_RIGHT
268 touch_direction = M_BACK
269 touch_state = 1
270 touch_dur = BEAT2
271 end if
272
273 if tch_fr = 0 then
274 touch_turn = M_LEFT
275 touch_direction = M_BACK
276 touch_state = 1
277 touch_dur = BEAT2
278 end if
279
280 select case touch_state
281
282 case 0:
283
284 case 1:
285 motor_vector = touch_direction
286 if touch_dur = 0 then
287 touch_state = 2
288 touch_dur = BEAT2
289 end if
290 decr touch_dur
291
292 case 2:
293 motor_vector = touch_turn
294 if touch_dur = 0 then
295 touch_state = 0
296 touch_dur = 0
297 motor_vector = M_STOP
298 end if
299 decr touch_dur
300
301 end select
302 end sub
303
304 sub photofoob
305 select case photofoob_state
306
307 case 0:
308 ' we kick into action when it is too light
309 if eye_l > photofoob_offset or eye_r > photofoob_offset then
310 photofoob_turn = M_LEFT
311 if eye_l > eye_r then
312 photofoob_turn = M_RIGHT
313 end if
314 photofoob_dur = BEAT
315 photofoob_state = 1
316 end if
317
318 case 1:
319 motor_vector = photofoob_turn
320 if photofoob_dur = 0 then
321 photofoob_state = 2
322 photofoob_dur = BEAT
323 end if
324 decr photofoob_dur
325
326 case 2:
327 motor_vector = M_FORWARD
328 if photofoob_dur = 0 then
329 photofoob_state = 0
330 motor_vector = M_STOP
331 end if
332 decr photofoob_dur
333
334 end select
335 end sub
336
337 sub wander
338 select case wander_state
339
340 case 0:
341 i = rnd(100)
342 i = i * 10
343 wander_dur = i * BEAT
344 wander_state = 1
345
346 case 1:
347 if wander_dur = 0 then
348 wander_state = 2
349 wander_turnorgo = 0
350 wander_dur = WANDER_RUN
351 end if
352 decr wander_dur
353
354 case 2:
355 if wander_turnorgo = 0 then
356 motor_vector = M_LEFT
357 if eye_l < eye_r then
358 motor_vector = M_RIGHT
359 end if
360 else
361 motor_vector = M_FORWARD
362 end if
363 if wander_dur = 0 then
364 wander_state = 0
365 motor_vector = M_STOP
366 end if
367 decr wander_dur
368 i = wander_dur mod BEAT2
369 if i = 0 then
370 toggle wander_turnorgo
371 end if
372
373 end select
374 end sub----------------------------------------
（编辑：hupo）
视频下载地址：http://www.robotsky.com/down/videos/2008-04-18/1208496477134.html
【