root/transterpreter/branches/surveyor/wrappers/surveyor/programs/subsumption.occ

Revision 2548, 5.7 kB (checked in by clj3, 1 year ago)

I think this fixes the playback code. Ie we were not replaying the correct motor
commands with the current time delta. We need to replay step (i-1) for time (i).
We could possibly also fix this by changing the way that things are recorded.

The problem was that a command is sent (say #i), this is stored with a timedelta
since the last command. However that timedelta has nothing to do with how long
command #i ran, but instead refers to the length of command #i-1. Hence the fix
above.

Line 
1 #OPTION "V"
2 #INCLUDE "surveyor.inc"
3
4 VAL INT SPEED IS 75:
5 VAL INT SECONDS IS 1000:
6 VAL INT HALFSEC IS 500:
7
8 PROC run.motor (CHAN [2]INT spd?)
9   [2]INT s:
10   WHILE TRUE
11     SEQ
12       spd ? s
13       ffi.set.speed(s[0], s[1])
14 :
15
16 PROC forward (CHAN [2]INT motor.spd!)
17   WHILE TRUE
18     motor.spd ! [SPEED, SPEED]
19 :
20
21 --VAL INT FWD.SPD IS 75:
22 VAL INT BUMP.THRESHOLD IS 45:
23 VAL INT TURN.DELAY IS 1500:
24 VAL INT FWD.TIME IS 30:
25 VAL INT MAXARR IS 1000:
26 --VAL INT REV.SPD IS -50:
27
28 -- This does not return a real distance. The higher the number the closer the
29 -- robot is to an obstacle.
30 PROC ir.dist(VAL INT dir, CHAN INT dists!)
31   WHILE TRUE
32     INT result:
33     SEQ
34       ffi.ir.bounce(dir, #FF, result)
35       dists ! result
36       INT t:
37       TIMER tim:
38       SEQ
39         tim ? t
40         tim ? AFTER t PLUS 100
41 :
42
43 PROC threshold(VAL INT t, CHAN INT dist?, CHAN BOOL fired!)
44   WHILE TRUE
45     INT d:
46     SEQ
47       dist ? d
48       IF
49         d > t
50           SEQ
51             fired ! TRUE
52             --dist ? d
53             WHILE d > t
54               dist ? d
55             fired ! TRUE
56         TRUE 
57           SKIP
58 :       
59          
60 PROC avoid.core(VAL INT spd, VAL INT turn.delay, CHAN BOOL alert?, CHAN [2]INT motor.spd!)
61   WHILE TRUE
62     BOOL dummy:
63     SEQ
64       motor.spd ! [spd, spd]
65       alert ? dummy
66       motor.spd ! [-spd, spd]
67       alert ? dummy
68 :
69
70 PROC avoid (CHAN [2]INT motor.spd!)
71   CHAN INT dists:
72   CHAN [2]INT spd:
73   CHAN BOOL bumped:
74   PAR
75     threshold(BUMP.THRESHOLD, dists?, bumped!)
76     avoid.core(SPEED, TURN.DELAY, bumped?, motor.spd!)
77     ir.dist(IR.FORWARD, dists!)
78 :
79
80 PROC printstuff(VAL []BYTE str, VAL INT i, start, end, delta, VAL [2]INT s)
81   SEQ
82     ffi.send.string(str)
83     ffi.send.string(" i: ")
84     ffi.send.number(i)
85     ffi.send.string(" ts ")
86     ffi.send.number(start)
87     ffi.send.string(" te: ")
88     ffi.send.number(end)
89     ffi.send.string(" td: ")
90     ffi.send.number(delta)
91     ffi.send.string(" 0: ")
92     ffi.send.number(s[0])
93     ffi.send.string(" 1: ")
94     ffi.send.number(s[1])
95     ffi.send.string("*c*n")
96 :
97
98
99 PROC rec (CHAN [2]INT spd.in?, CHAN BOOL play.req?, CHAN [3]INT play.rsp!)
100   [2]INT s:
101   [MAXARR][3]INT store:
102   INITIAL INT i IS 0:
103   TIMER tim:
104   INT start, end:
105   BOOL b:
106   INITIAL BOOL first IS TRUE:
107   SEQ
108     printstuff("Storing (0)", i, 0, 0, 0, [0, 0])
109     store[i][0] := 0
110     store[i][1] := 0
111     store[i][2] := 0
112     i := i + 1
113     tim ? start
114     WHILE TRUE
115       PRI ALT
116         play.req ? b
117           SEQ
118             IF
119               first
120                 SEQ
121                   tim ? end
122                   store[i][0] := s[0]
123                   store[i][1] := s[1]
124                   store[i][2] := end - start
125                   first := FALSE
126                   i := i + 1
127               TRUE
128                 SKIP
129             IF
130               (i = 1)
131                 SEQ
132                   ffi.send.string("All done!")
133                   ffi.set.speed(0,0)
134                   STOP
135               TRUE
136                 SEQ
137                   i := i - 1
138                   printstuff("Replaying", i, 0, 0, store[i][2], [store[i][0], store[i][1]])
139                   play.rsp ! [store[i - 1][0], store[i - 1][1], store[i][2]]
140         spd.in ? s
141           SEQ
142             tim ? end
143             IF
144               ((store[i - 1][0] <> s[0]) OR (store[i - 1][1] <> s[1]))
145                 SEQ
146                   printstuff("Storing (x)", i, start, end, end - start, s)
147                   store[i][0] := s[0]
148                   store[i][1] := s[1]
149                   store[i][2] := end - start
150                   start := end
151                   IF
152                     i = (MAXARR - 1)
153                       SEQ
154                         ffi.send.string("Too much data!*c*n")
155                         i := 0
156                     TRUE
157                       i := i + 1
158               TRUE
159                 SKIP
160 :
161
162 PROC replay (CHAN BOOL play.req!, CHAN [3]INT play.rsp?, CHAN [2]INT motor.spd!, CHAN BOOL rep2)
163   TIMER tim:
164   INT t:
165   [3]INT stored:
166   SEQ
167     tim ? t
168     tim ? AFTER (FWD.TIME * SECONDS)
169     ffi.send.string("Starting replay*c*n")
170     WHILE TRUE
171       SEQ
172         rep2 ! TRUE
173         play.req ! TRUE
174         play.rsp ? stored
175         motor.spd ! [-stored[0], -stored[1]]
176         tim ? t
177         tim ? AFTER t PLUS stored[2]
178 :
179
180 PROC suppressor (VAL INT time, CHAN [2]INT suppress?, in?, out!)
181   TIMER tim:
182   INITIAL INT timeout IS 0:
183   INITIAL BOOL suppressing IS FALSE:
184   [2]INT value:
185   WHILE TRUE
186     PRI ALT
187       NOT suppressing & suppress ? value
188         SEQ
189           suppressing := TRUE
190           tim ? timeout
191           timeout := timeout PLUS time
192           out ! value
193       NOT suppressing & in ? value
194         out ! value
195       suppressing & tim? AFTER timeout
196         suppressing := FALSE
197       suppressing & suppress ? value
198         out ! value
199 :
200
201 PROC inhibitor (VAL INT time, CHAN BOOL inhibit, CHAN [2]INT in?, out!)
202   TIMER tim:
203   INITIAL INT timeout IS 0:
204   INITIAL BOOL inhibiting IS FALSE:
205   [2]INT data:
206   BOOL flag:
207   WHILE TRUE
208     PRI ALT
209       inhibit ? flag
210         SEQ
211           inhibiting := TRUE
212           tim ? timeout
213           timeout := timeout PLUS time
214       inhibiting & tim? AFTER timeout
215         inhibiting := FALSE
216       NOT inhibiting & in ? data
217         out ! data
218       inhibiting & in ? data
219         SKIP
220 :
221
222 PROC delta(CHAN [2]INT in?, out.1!, out.2!)
223   [2]INT x:
224   WHILE TRUE
225     SEQ
226       in ? x
227       out.1 ! x
228       out.2 ! x
229 :
230
231 PROC main (CHAN BYTE k, s, e)
232   CHAN [2]INT go, av, rep, recgo, recgo2, s1, s2, m:
233   CHAN BOOL req, rep2:
234   CHAN [3]INT rsp:
235   PAR
236     forward(go!)
237     avoid(av!)
238     suppressor(2 * SECONDS, av?, go?, s1!)
239     suppressor((MOSTPOS INT), rep, s1?, s2!)
240     delta(s2?, m!, recgo!)
241     inhibitor((MOSTPOS INT), rep2?, recgo?, recgo2!)
242     run.motor(m?)
243     rec(recgo2?, req?, rsp!)
244     replay(req!, rsp?, rep!, rep2!)
245 :
Note: See TracBrowser for help on using the browser.