root/kroc/trunk/tvm/srv1/programs/srv1.occ

Revision 4824, 7.4 KB (checked in by cgr, 2 years ago)

Provide synchronous and asynchronous bulk output.
Line 
1#INCLUDE "srv1.inc"
2#INCLUDE "delay.inc"
3#INCLUDE "out.occ"
4
5PROC drain.uart (CHAN BYTE in?)
6  INITIAL INT clear IS 0:
7  WHILE clear < 10
8    PRI ALT
9      BYTE b:
10      in ? b
11        SKIP
12      TRUE & SKIP
13        clear := clear + 1
14:
15
16PROC protocol.error (VAL BYTE prefix, ch, RESULT BOOL custom.response, CHAN BYTE out!)
17  SEQ
18    custom.response := TRUE
19    out.string("Invalid command ", 0, out!)
20    IF
21      prefix = 0
22        SKIP
23      TRUE
24        out ! prefix
25    out ! ch
26    out.string(" found in protocol*n", 0, out!)
27:
28
29PROC srv1 (CAMERA! camera, CONSOLE! console, LASER! lasers, MOTOR! motors, SYSTEM! system)
30  INITIAL [3]BYTE response IS ['#', 0, '*n']:
31  INITIAL INT frame.count IS 0:
32  INITIAL INT width IS 320:
33  INITIAL INT height IS 256:
34  INITIAL BOOL overlay IS TRUE:
35  INITIAL INT quality IS 3:
36  SEQ
37    camera[req] ! set.stream.mode; 320; 256
38    camera[rsp] ? CASE ok
39
40    WHILE TRUE
41      INITIAL BOOL custom.response IS FALSE:
42      BYTE ch:
43      SEQ
44        console[in] ? ch
45        response[1] := ch
46        CASE ch
47          -- Direct motor control
48          'M'
49            SKIP
50          -- Drift left
51          '7'
52            motors[req] ! turn; 15
53          -- Drive forward
54          '8'
55            motors[req] ! all; 50; 50
56          -- Drift right
57          '9'
58            motors[req] ! turn; -15
59          -- Drive left
60          '4'
61            motors[req] ! turn; 30
62          -- Stop
63          '5'
64            motors[req] ! all; 50; 50
65          -- Drive right
66          '6'
67            motors[req] ! turn; -30
68          -- Back left
69          '1'
70            motors[req] ! back.turn; 30
71          -- Reverse
72          '2'
73            motors[req] ! delta; -1; -1
74          -- Back right
75          '3'
76            motors[req] ! back.turn; -30
77          -- Clockwise turn
78          '.'
79            SEQ
80              motors[req] ! turn; 75
81              delay.ms(200)
82              motors[req] ! stop
83          -- High motor speed range
84          '+'
85            SKIP
86          -- Low motor speed range
87          '-'
88            SKIP
89          -- Set camera resolution to 160x128
90          'a'
91            SEQ
92              width, height := 160, 128
93              camera[req] ! set.stream.mode; width; height
94              camera[rsp] ? CASE ok
95          -- Set camera resolution to 320x256
96          'b'
97            SEQ
98              width, height := 320, 256
99              camera[req] ! set.stream.mode; width; height
100              camera[rsp] ? CASE ok
101          -- Set camera resolution to 640x512
102          'c'
103            SEQ
104              width, height := 640, 512
105              camera[req] ! set.stream.mode; width; height
106              camera[rsp] ? CASE ok
107          -- Set camera resolution to 1280x1024
108          'A'
109            SEQ
110              width, height := 1280, 1024
111              camera[req] ! set.capture.mode; width; height
112              camera[rsp] ? CASE ok
113          -- Grab JPEG compressed video frame
114          'I'
115            MOBILE []BYTE frame, jpeg.data:
116            INITIAL [10]BYTE header IS "##IMJ00000":
117            INT jpeg.bytes:
118            SEQ
119              camera[req] ! get.frame
120              camera[rsp] ? CASE frame; frame
121              IF
122                overlay
123                  INITIAL [9]BYTE caption IS "frame 000":
124                  SEQ
125                    caption[6] := BYTE ('0' + (((frame.count / 100) \ 10)))
126                    caption[7] := BYTE ('0' + (((frame.count / 10) \ 10)))
127                    caption[8] := BYTE ('0' +  (frame.count \ 10))
128                    draw.caption.on.frame (width, caption, frame)
129                TRUE
130                  SKIP
131              jpeg.data := MOBILE [(SIZE frame) >> 2]BYTE
132              jpeg.encode.frame (width, height, quality, frame, jpeg.data, jpeg.bytes)
133              CASE width
134                80
135                  header[5] := '1'
136                160
137                  header[5] := '3'
138                320
139                  header[5] := '5'
140                640
141                  header[5] := '7'
142                1280
143                  header[5] := '9'
144             
145              [4]BYTE size RETYPES jpeg.bytes:
146              SEQ i = 0 FOR 4
147                header[6 + i] := size[i]
148              SEQ i = 0 FOR SIZE header
149                console[out] ! header[i]
150
151              console[cmd.req] ! bulk.out; FALSE; jpeg.bytes; jpeg.data
152
153              drain.uart (console[in]?)
154
155              frame.count := frame.count + 1
156              custom.response := TRUE
157          -- Camera overlay
158          'o'
159            overlay := TRUE
160          'O'
161            overlay := FALSE
162          -- Lasers
163          'l'
164            lasers[req] ! all; TRUE  -- On
165          'L'
166            lasers[req] ! all; FALSE -- Off
167          -- Time in MS since reset
168          't'
169            SKIP
170          -- JPEG Quality
171          'q'
172            SEQ
173              console[in] ? ch
174              quality := INT (ch - '0')
175              IF
176                quality < 1
177                  quality := 1
178                quality > 8
179                  quality := 8
180                TRUE
181                  SKIP
182              out.string ("##quality ", 0, console[out]!)
183              out.int (quality, 0, console[out]!)
184              console[out] ! '*n'
185              custom.response := TRUE
186          -- Range in centimeters to nearest object
187          'R'
188            SKIP
189          -- Version
190          'V'
191            SKIP
192          -- IO pins
193          'd'
194            SEQ
195              console[in] ? ch
196              CASE ch
197                'd'
198                  SKIP -- Direction
199                'r'
200                  SKIP -- Read
201                'w'
202                  SKIP -- Write
203                ELSE
204                  protocol.error('d', ch, custom.response, console[out]!)
205          -- Xmodem recieve file
206          'X'
207            SKIP
208          -- Flash Memory
209          'z'
210            SEQ
211              console[in] ? ch
212              CASE ch
213                'w'
214                  SKIP -- Write
215                'r'
216                  SKIP -- Read
217                'd'
218                  SKIP -- Dump
219                'Z'
220                  SKIP -- Update boot sector
221                ELSE
222                  protocol.error('z', ch, custom.response, console[out]!)
223          -- Vision commands
224          'v'
225            SEQ
226              console[in] ? ch
227              CASE ch
228                'g'
229                  SKIP -- Grab and sample region
230                'r'
231                  SKIP -- Retrieve stored color info from color bin
232                'c'
233                  SKIP -- Set the contents of a color bin
234                's'
235                  SKIP -- View pixel column vector data from color bin
236                'f'
237                  SKIP -- Distance to first pixel matching target color
238                'b'
239                  SKIP -- Search for blobs
240                'n'
241                  SKIP -- Blobs, returns the number of pixels matching target color
242                ELSE
243                  protocol.error('v', ch, custom.response, console[out]!)
244          'Q'
245            SKIP -- Execute C program stored in flash memory.
246          -- Carriage Return
247          '*n'
248            custom.response := TRUE -- Ignore, makes netcat/telnet happy.
249          ELSE
250            protocol.error(0, ch, custom.response, console[out]!)
251        -- Respond to commands, if appropriate.
252        IF
253          NOT custom.response
254            out.string(response, 0, console[out]!)
255          TRUE
256            SKIP
257:
Note: See TracBrowser for help on using the browser.