Rev 1086 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1085 | pj | 1 | /*--------------------------------------------------------------*/ |
2 | /* Title: Mouse */ |
||
3 | /* Autor: Joćo Capucho */ |
||
4 | /* Date: 9/12/2000 */ |
||
5 | /* Description: */ |
||
6 | /* Simulation of small automats based on the */ |
||
7 | /* "micro-rato" conteste */ |
||
8 | /*--------------------------------------------------------------*/ |
||
9 | |||
10 | /* |
||
11 | * Copyright (C) 2001 Joćo Capucho |
||
12 | * |
||
13 | * This program is free software; you can redistribute it and/or modify |
||
14 | * it under the terms of the GNU General Public License as published by |
||
15 | * the Free Software Foundation; either version 2 of the License, or |
||
16 | * (at your option) any later version. |
||
17 | * |
||
18 | * This program is distributed in the hope that it will be useful, |
||
19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
21 | * GNU General Public License for more details. |
||
22 | * |
||
23 | * You should have received a copy of the GNU General Public License |
||
24 | * along with this program; if not, write to the Free Software |
||
25 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
26 | * |
||
27 | */ |
||
28 | |||
29 | |||
30 | // Includes |
||
31 | #include <ll/i386/x-dos.h> |
||
32 | #include <kernel/kern.h> |
||
33 | #include <drivers/glib.h> |
||
34 | #include <drivers/keyb.h> |
||
35 | #include <math.h> |
||
36 | #include <stdlib.h> |
||
37 | #include <limits.h> |
||
38 | |||
39 | #define sqr(x) (x*x) |
||
40 | |||
41 | // Error Types |
||
42 | #define OK 0 |
||
43 | #define MEMORY_ERROR -1 |
||
44 | #define ILEGAL_PARAMETER -2 |
||
45 | |||
46 | // Types of sensors and motors |
||
47 | #define NONE 0 |
||
48 | #define OBSTACLE 1 |
||
49 | #define BEACON 2 |
||
50 | #define GROUND 3 |
||
51 | #define STEP 4 |
||
52 | |||
53 | // Maze limits |
||
54 | #define LAB_XMIN 40 |
||
55 | #define LAB_XMAX 600 |
||
56 | #define LAB_YMIN 40 |
||
57 | #define LAB_YMAX 440 |
||
58 | |||
59 | #define MOTOR_RIGHT 1 |
||
60 | #define MOTOR_LEFT 0 |
||
61 | |||
62 | #define BACKCOLOR 0 // Background color |
||
63 | #define OUTCOLOR 14 // Color of the outside walls |
||
64 | #define FINISHCOLOR 7 // Finishin area color |
||
65 | #define NONBLOCKCOLOR 14 // Non block boxs |
||
66 | #define BLOCKCOLOR 13 // Block boxs |
||
67 | |||
68 | #define BOX_BLOCK 1 // The box blocks the beacon |
||
69 | #define BOX_NON_BLOCK 2 // The box doesn“t block the beacon |
||
70 | |||
71 | #define MAX_MOUSES 5 // Maximum of mouses in the Maze |
||
72 | #define MAX_BOXS 100 // Maximum of boxs in the Maze |
||
73 | #define MAX_SENSORS 20 // Maximum of sensors in each mouse |
||
74 | #define SENSOR_MIN_GROUND 0 // Minimum value of a ground sensor |
||
75 | #define SENSOR_MAX_GROUND 255 // Maximum value of a ground sensor |
||
76 | #define SENSOR_MIN_OBSTACLE 77 // Minimum value of a obstacle sensor |
||
77 | #define SENSOR_MAX_OBSTACLE 128 // Maximum value of a obstacle sensor |
||
78 | #define SENSOR_MIN_BEACON 77 // Minimum value of a beacon sensor |
||
79 | #define SENSOR_MAX_BEACON 128 // Maximum value of a beacon sensor |
||
80 | #define BEACON_INTENSITY 400 // Maximun intensity of the beacon |
||
81 | |||
82 | #define MOUSE_PERIOD 20000 // Period |
||
83 | #define CALC_PERIOD 2000 // Period |
||
84 | #define REFEREE_PERIOD 1000 // Period of the referee |
||
85 | #define REDRAW_MOUSE_PERIOD 25000 // Period for redraw mouse |
||
86 | #define REDRAW_MAZE_PERIOD 100000 // Period for redraw maze |
||
87 | #define MOUSEGROUP 511 |
||
88 | |||
89 | struct SENSOR |
||
90 | { |
||
91 | int Type; // Tipe of sensor ( None - Obstacle - Beacon - Ground) |
||
92 | int Radius; // Distance from de center of the mouse |
||
93 | int Pos_Angle; // Angle between the sensor and the front of the mouse |
||
94 | int Dir; // Angle between the direction of the sensores and the front of the mouse |
||
95 | int Angle; // Half angle of visual field (between (Dir-Angle) and (Dir+Angle)) |
||
96 | int Intensity; // Sensativity of the sensor |
||
97 | float Value; // Last value readed |
||
98 | }; |
||
99 | |||
100 | struct MOTOR |
||
101 | { |
||
102 | int Radius; // Distance from de center of the mouse |
||
103 | int V; // Current velocity |
||
104 | float Old_V; // Previews velocity |
||
105 | }; |
||
106 | |||
107 | struct MOUSE |
||
108 | { |
||
109 | float X, Y; // Current position of the center |
||
110 | float Old_X, Old_Y; // Previous position of the center |
||
111 | int Radius; // Size of the mouse ( every mouse is considered round ) |
||
112 | float Dir; // Points to the front |
||
113 | float Old_Dir; // Previous front |
||
114 | int NSensors; // Number of sensors in the mouse |
||
115 | struct SENSOR Sensors[MAX_SENSORS]; // Pointer to the structer that contains the information of the sensors |
||
116 | struct MOTOR Motors[2]; // Pointer to the structer that contains the information of the motors |
||
117 | WORD NColision; // Total of colisions |
||
118 | WORD PColision; // Auxiliar for colisions |
||
119 | char Flag; // Initialization flag |
||
120 | }; |
||
121 | |||
122 | |||
123 | |||
124 | struct BOX // Structer that represents the obstacles |
||
125 | { //in this case all the obstacles are boxs |
||
126 | int Type; // Type of the obstatcle |
||
127 | int X1, Y1, X2, Y2; // position of the box |
||
128 | }; |
||
129 | |||
130 | struct SBEACON // Beacon information |
||
131 | { |
||
132 | int Intensity; // Intensity of the beacon |
||
133 | int X, Y; // position of the beacon |
||
134 | }; |
||
135 | |||
136 | struct MAZE |
||
137 | { |
||
138 | int X1, Y1, X2, Y2; // limites of the maze |
||
139 | int NBoxs; // Number of objstacles in the maze |
||
140 | struct BOX Boxs[MAX_BOXS]; // Information about all thr obstacles |
||
141 | struct SBEACON Beacon; // Beacon data |
||
142 | int Radius; // Size of the finishing area |
||
143 | }; |
||
144 | |||
145 | int Seno[360+90]; // Sine table |
||
146 | int Tang[180]; // Tangente table |
||
147 | int NMouses=0; |
||
148 | struct MOUSE Mouse[MAX_MOUSES]; // Global variable that represents all the mouses |
||
149 | struct MAZE Maze; // Global variable that represents the maze |
||
150 | |||
151 | // Init_Maze - Initialize the Maze |
||
152 | // Beacon_X - Coordinate X of the beacon |
||
153 | // Beacon_Y - Coordinate Y of the beacon |
||
154 | // Intensity - Intensity of the beacon |
||
155 | // NBoxs - Number of boxs in the maze |
||
156 | // Return: OK - Sucess |
||
157 | |||
158 | int Init_Maze(int Beacon_X, int Beacon_Y, int Intensity, int NBoxs) |
||
159 | { |
||
160 | int i; |
||
161 | Maze.X1=LAB_XMIN; |
||
162 | Maze.Y1=LAB_YMIN; |
||
163 | Maze.X2=LAB_XMAX; |
||
164 | Maze.Y2=LAB_YMAX; |
||
165 | Maze.Beacon.X=Beacon_X+LAB_XMIN; |
||
166 | Maze.Beacon.Y=Beacon_Y+LAB_YMIN; |
||
167 | if (Intensity<BEACON_INTENSITY) |
||
168 | Maze.Beacon.Intensity=Intensity; |
||
169 | else |
||
170 | Maze.Beacon.Intensity=BEACON_INTENSITY; |
||
171 | if (NBoxs<MAX_BOXS) |
||
172 | Maze.NBoxs=NBoxs; |
||
173 | else |
||
174 | Maze.NBoxs=MAX_BOXS; |
||
175 | Maze.Radius=50; |
||
176 | for(i=0; i < Maze.NBoxs; i++) |
||
177 | Maze.Boxs[i].Type=NONE; |
||
178 | return OK; |
||
179 | } |
||
180 | |||
181 | // Init_Box - Initialize data of the NBox obstacle |
||
182 | // NBox - Identifier of the obstacle |
||
183 | // X1 - Coordinate X |
||
184 | // Y1 - Coordinate Y |
||
185 | // X2 - Coordinate X |
||
186 | // Y2 - Coordinate Y |
||
187 | // Type - Type of the obstacle |
||
188 | // Return: OK - Sucess |
||
189 | |||
190 | int Init_Box(int NBox, int X1, int Y1, int X2, int Y2, int Type) |
||
191 | { |
||
192 | Maze.Boxs[NBox].Type=Type; |
||
193 | if (X1<X2) |
||
194 | { |
||
195 | Maze.Boxs[NBox].X1=X1+LAB_XMIN; |
||
196 | Maze.Boxs[NBox].X2=X2+LAB_XMIN; |
||
197 | } |
||
198 | else |
||
199 | { |
||
200 | Maze.Boxs[NBox].X1=X2+LAB_XMIN; |
||
201 | Maze.Boxs[NBox].X2=X1+LAB_XMIN; |
||
202 | } |
||
203 | if (Y1<Y2) |
||
204 | { |
||
205 | Maze.Boxs[NBox].Y1=Y1+LAB_YMIN; |
||
206 | Maze.Boxs[NBox].Y2=Y2+LAB_YMIN; |
||
207 | } |
||
208 | else |
||
209 | { |
||
210 | Maze.Boxs[NBox].Y1=Y2+LAB_YMIN; |
||
211 | Maze.Boxs[NBox].Y2=Y1+LAB_YMIN; |
||
212 | } |
||
213 | return OK; |
||
214 | } |
||
215 | |||
216 | // Draw_Maze - Draws the outline of the maze and the obstacles |
||
217 | // Return: OK - Sucess |
||
218 | int Draw_Maze(void) |
||
219 | { |
||
220 | int i; |
||
221 | grx_rect( Maze.X1, Maze.Y1, Maze.X2, Maze.Y2, OUTCOLOR); |
||
222 | grx_rect( Maze.X1-1, Maze.Y1+1, Maze.X2+1, Maze.Y2+1, OUTCOLOR); |
||
223 | grx_rect( Maze.X1-2, Maze.Y1+2, Maze.X2+2, Maze.Y2+2, OUTCOLOR); |
||
224 | grx_circle(Maze.Beacon.X, Maze.Beacon.Y, Maze.Radius, FINISHCOLOR); |
||
225 | grx_disc(Maze.Beacon.X, Maze.Beacon.Y, 10, 7); |
||
226 | for (i=0; i<Maze.NBoxs; i++) |
||
227 | { |
||
228 | if (Maze.Boxs[i].Type==BOX_NON_BLOCK) |
||
229 | grx_box(Maze.Boxs[i].X1, Maze.Boxs[i].Y1, |
||
230 | Maze.Boxs[i].X2, Maze.Boxs[i].Y2, NONBLOCKCOLOR); |
||
231 | if (Maze.Boxs[i].Type==BOX_BLOCK) |
||
232 | grx_box(Maze.Boxs[i].X1, Maze.Boxs[i].Y1, |
||
233 | Maze.Boxs[i].X2, Maze.Boxs[i].Y2, BLOCKCOLOR); |
||
234 | } |
||
235 | return OK; |
||
236 | } |
||
237 | |||
238 | int Init_Motor(int NMouse, int Motor, int Radius) |
||
239 | { |
||
240 | if ((NMouse<0) || (NMouse>=MAX_MOUSES)) |
||
241 | return ILEGAL_PARAMETER; |
||
242 | if ((Motor<0) || (Motor>=2)) |
||
243 | return ILEGAL_PARAMETER; |
||
244 | Mouse[NMouse].Motors[Motor].Radius=Radius; |
||
245 | Mouse[NMouse].Motors[Motor].V=0; |
||
246 | Mouse[NMouse].Motors[Motor].Old_V=0.0; |
||
247 | return OK; |
||
248 | } |
||
249 | |||
250 | // Init_Mouse - inicialize the parameters of each mouse |
||
251 | // NMouse - Identifier of the mouse |
||
252 | // X - Coordinate X of the center |
||
253 | // Y - Coordinate Y of the center |
||
254 | // Radius - Radius of the mouse (size) |
||
255 | // Dir - Angle of direction in witch i will start |
||
256 | // NSensors - Number of sensors in the mouse (all types) |
||
257 | // |
||
258 | // Return: OK - Sucess |
||
259 | // ILEGAL_PARAMETER - If NMouse negative of greater than maximum number of mouses |
||
260 | // Note: All the types of sensors and motors are initialize to NONE |
||
261 | |||
262 | int Init_Mouse(int NMouse, int X, int Y, int Radius, int Dir, int NSensors) |
||
263 | { |
||
264 | int i; |
||
265 | if ((NMouse<0) || (NMouse>=MAX_MOUSES)) |
||
266 | return ILEGAL_PARAMETER; |
||
267 | if ((NSensors<0) || (NSensors>MAX_SENSORS)) |
||
268 | return ILEGAL_PARAMETER; |
||
269 | Mouse[NMouse].X=(float)X+LAB_XMIN; |
||
270 | Mouse[NMouse].Y=(float)Y+LAB_YMIN; |
||
271 | Mouse[NMouse].Radius=Radius; |
||
272 | Mouse[NMouse].Dir=Dir; |
||
273 | Mouse[NMouse].Old_Dir=Mouse[NMouse].Dir; |
||
274 | Mouse[NMouse].NSensors=NSensors; |
||
275 | Mouse[NMouse].Old_X=Mouse[NMouse].X; |
||
276 | Mouse[NMouse].Old_Y=Mouse[NMouse].Y; |
||
277 | Mouse[NMouse].NColision=0; |
||
278 | Mouse[NMouse].PColision=0; |
||
279 | for(i=0; i < Mouse[NMouse].NSensors; i++) |
||
280 | Mouse[NMouse].Sensors[i].Type=NONE; |
||
281 | |||
282 | Init_Motor(NMouse, MOTOR_LEFT, 10); |
||
283 | Init_Motor(NMouse, MOTOR_RIGHT, -10); |
||
284 | |||
285 | return OK; |
||
286 | } |
||
287 | |||
288 | // Init_Sensor - Initializes the sensor information |
||
289 | // NMouse - Identifier of the mouse |
||
290 | // Sensor - Identifier of the sensor to be used |
||
291 | // Type - Type of sensor ( Ground, obstacle or beacon sensor ) |
||
292 | // Radius and Pos_Angle - Polar coordinates to determen the sensor position |
||
293 | // Dir - Diretion of the sensor ( angle between the front and the sensor ) |
||
294 | // Angle - Half of the visual range |
||
295 | // Intensity - Maximun distance of detect |
||
296 | int Init_Sensor(int NMouse, int Sensor, int Type, int Radius, |
||
297 | int Pos_Angle, int Dir, int Angle, int Intensity) |
||
298 | { |
||
299 | if ((NMouse<0) || (NMouse>=MAX_MOUSES)) |
||
300 | return ILEGAL_PARAMETER; |
||
301 | if ((Sensor<0) || (Sensor>=Mouse[NMouse].NSensors)) |
||
302 | return ILEGAL_PARAMETER; |
||
303 | Mouse[NMouse].Sensors[Sensor].Type=Type; |
||
304 | Mouse[NMouse].Sensors[Sensor].Radius=Radius; |
||
305 | Mouse[NMouse].Sensors[Sensor].Pos_Angle=Pos_Angle; |
||
306 | Mouse[NMouse].Sensors[Sensor].Dir=Dir; |
||
307 | Mouse[NMouse].Sensors[Sensor].Angle=Angle; |
||
308 | Mouse[NMouse].Sensors[Sensor].Intensity=Intensity; |
||
309 | if (Type == GROUND) |
||
310 | Mouse[NMouse].Sensors[Sensor].Value=(float)SENSOR_MIN_GROUND; |
||
311 | if (Type == OBSTACLE) |
||
312 | Mouse[NMouse].Sensors[Sensor].Value=(float)SENSOR_MIN_OBSTACLE; |
||
313 | if (Type == BEACON) |
||
314 | Mouse[NMouse].Sensors[Sensor].Value=(float)SENSOR_MIN_BEACON; |
||
315 | return OK; |
||
316 | } |
||
317 | |||
318 | int Set_Motor_Vel(int NMouse, int NMotor, int Vel) |
||
319 | { |
||
320 | if ((NMouse<0) || (NMouse>=MAX_MOUSES)) |
||
321 | return ILEGAL_PARAMETER; |
||
322 | if ((NMotor<0) || (NMotor>2)) |
||
323 | return ILEGAL_PARAMETER; |
||
324 | Mouse[NMouse].Motors[NMotor].V=Vel; |
||
325 | return OK; |
||
326 | } |
||
327 | |||
328 | int Read_Sensor(int NMouse, int NSensor) |
||
329 | { |
||
330 | int X, Y, Dir, cr, sr, AuxX, AuxY; |
||
331 | int Count, AuxCount, AngCount, AuxAngCount, RetColor, Intensity; |
||
332 | float Auxf; |
||
333 | |||
334 | /* Inicial verifications */ |
||
335 | if ((NMouse<0) || (NMouse>=MAX_MOUSES)) |
||
336 | return ILEGAL_PARAMETER; |
||
337 | if ((NSensor<0) || (NSensor>Mouse[NMouse].NSensors)) |
||
338 | return ILEGAL_PARAMETER; |
||
339 | |||
340 | /* If Sensor Not define exit */ |
||
341 | if ( Mouse[NMouse].Sensors[NSensor].Type == NONE ) |
||
342 | return OK; |
||
343 | |||
344 | /* Calculate de position of the sensor and is direction */ |
||
345 | Dir=(int)Mouse[NMouse].Old_Dir+Mouse[NMouse].Sensors[NSensor].Dir; |
||
346 | if (Dir >= 360) |
||
347 | Dir -= 360; |
||
348 | if (Dir < 0) |
||
349 | Dir += 360; |
||
350 | X= Mouse[NMouse].Old_X*65536.0+((Mouse[NMouse].Sensors[NSensor].Radius*Seno[Dir+90])); |
||
351 | Y= Mouse[NMouse].Old_Y*65536.0+((Mouse[NMouse].Sensors[NSensor].Radius*Seno[Dir])); |
||
352 | |||
353 | /* if the Sensor type is OBSTACLE */ |
||
354 | if ( Mouse[NMouse].Sensors[NSensor].Type == OBSTACLE ) |
||
355 | { |
||
356 | Intensity=Mouse[NMouse].Sensors[NSensor].Intensity; |
||
357 | AuxCount=Intensity; |
||
358 | /* Scan all the visual field */ |
||
359 | for (AngCount=(-Mouse[NMouse].Sensors[NSensor].Angle); |
||
360 | AngCount<=Mouse[NMouse].Sensors[NSensor].Angle; |
||
361 | AngCount++) |
||
362 | { |
||
363 | AuxAngCount=Dir+AngCount; |
||
364 | if (AuxAngCount >= 360) |
||
365 | AuxAngCount -= 360; |
||
366 | if (AuxAngCount < 0) |
||
367 | AuxAngCount += 360; |
||
368 | sr=Seno[AuxAngCount]; |
||
369 | cr=Seno[AuxAngCount+90]; |
||
370 | Count=Mouse[NMouse].Radius-Mouse[NMouse].Sensors[NSensor].Radius+2; |
||
371 | AuxX = X+cr*Count; |
||
372 | AuxY = Y+sr*Count; |
||
373 | for (; Count<=AuxCount; Count++) |
||
374 | { |
||
375 | AuxX += cr; |
||
376 | AuxY += sr; |
||
377 | RetColor = grx_getpixel(AuxX >> 16, AuxY >> 16); |
||
378 | if ((RetColor != BACKCOLOR)&&(RetColor != FINISHCOLOR)) |
||
379 | AuxCount=Count; |
||
380 | } |
||
381 | } |
||
382 | Count=Mouse[NMouse].Sensors[NSensor].Intensity; |
||
383 | Count*=Count; |
||
384 | AuxCount*=AuxCount; |
||
385 | Count = SENSOR_MAX_OBSTACLE-((SENSOR_MAX_OBSTACLE-SENSOR_MIN_OBSTACLE) |
||
386 | *AuxCount)/Count; |
||
387 | Mouse[NMouse].Sensors[NSensor].Value= |
||
388 | (0.5*Mouse[NMouse].Sensors[NSensor].Value+0.5*(float)Count); |
||
389 | } |
||
390 | if ( Mouse[NMouse].Sensors[NSensor].Type == GROUND ) |
||
391 | { |
||
392 | Mouse[NMouse].Sensors[NSensor].Value=(float)SENSOR_MIN_GROUND; |
||
393 | AuxX=abs((X>>16)-Maze.Beacon.X); |
||
394 | AuxY=abs((Y>>16)-Maze.Beacon.Y); |
||
395 | if ((AuxX>50) || (AuxY>50)) |
||
396 | return OK; |
||
397 | if ((AuxX*AuxX+AuxY*AuxY)<Maze.Radius*Maze.Radius) |
||
398 | Mouse[NMouse].Sensors[NSensor].Value=(float)SENSOR_MAX_GROUND; |
||
399 | } |
||
400 | |||
401 | if ( Mouse[NMouse].Sensors[NSensor].Type == BEACON ) |
||
402 | { |
||
403 | AuxX=(X>>16)-Maze.Beacon.X; |
||
404 | AuxY=(Y>>16)-Maze.Beacon.Y; |
||
405 | AngCount=0; |
||
406 | if (AuxX!=0) |
||
407 | { |
||
408 | AuxAngCount=(AuxY<<16)/AuxX; |
||
409 | if (AuxAngCount<0) |
||
410 | AngCount=90; |
||
411 | for (Count=AngCount;Count<AngCount+90;Count++) |
||
412 | { |
||
413 | if ((Tang[Count]<=AuxAngCount) && (Tang[Count+1]>AuxAngCount)) |
||
414 | break; |
||
415 | } |
||
416 | } |
||
417 | else |
||
418 | Count=90; |
||
419 | if (AuxY<0) |
||
420 | Count+=180; |
||
421 | AuxAngCount=abs(180-abs(Dir-Count)); |
||
422 | Count=SENSOR_MIN_BEACON; |
||
423 | if (AuxAngCount<Mouse[NMouse].Sensors[NSensor].Angle) |
||
424 | { |
||
425 | Count=AuxX*AuxX+AuxY*AuxY; |
||
426 | AuxCount=(int)((float)Maze.Beacon.Intensity*(1.0+(0.4*(float)AuxAngCount)/(float)Mouse[NMouse].Sensors[NSensor].Angle)); |
||
427 | AuxCount*=AuxCount; |
||
428 | if (Count<AuxCount) |
||
429 | Count = SENSOR_MAX_OBSTACLE-((SENSOR_MAX_OBSTACLE-SENSOR_MIN_OBSTACLE)*Count)/AuxCount; |
||
430 | else |
||
431 | Count=SENSOR_MIN_BEACON; |
||
432 | } |
||
433 | Auxf=0.5*((float)Count); |
||
434 | Auxf+=0.5*((float)Mouse[NMouse].Sensors[NSensor].Value); |
||
435 | Mouse[NMouse].Sensors[NSensor].Value=Auxf; |
||
436 | } |
||
437 | return OK; |
||
438 | } |
||
439 | |||
440 | int GetSensor(int NMouse, int NSensor) |
||
441 | { |
||
442 | if ((NMouse<0) || (NMouse>=MAX_MOUSES)) |
||
443 | return ILEGAL_PARAMETER; |
||
444 | if ((NSensor<0) || (NSensor>Mouse[NMouse].NSensors)) |
||
445 | return ILEGAL_PARAMETER; |
||
446 | return (int)Mouse[NMouse].Sensors[NSensor].Value; |
||
447 | } |
||
448 | |||
449 | int Draw_Mouse(int NMouse, int Flag) |
||
450 | { |
||
451 | int Color; |
||
452 | if (!Flag) |
||
453 | Color=BACKCOLOR; |
||
454 | else |
||
455 | { |
||
456 | Mouse[NMouse].Old_X=Mouse[NMouse].X; |
||
457 | Mouse[NMouse].Old_Y=Mouse[NMouse].Y; |
||
458 | Mouse[NMouse].Old_Dir=Mouse[NMouse].Dir; |
||
459 | Color=2; |
||
460 | } |
||
461 | grx_circle(Mouse[NMouse].Old_X, Mouse[NMouse].Old_Y, Mouse[NMouse].Radius, Color); |
||
462 | grx_line(Mouse[NMouse].Old_X, Mouse[NMouse].Old_Y, |
||
463 | Mouse[NMouse].Old_X+((Mouse[NMouse].Radius*Seno[(int)Mouse[NMouse].Old_Dir+90]) >> 16), |
||
464 | Mouse[NMouse].Old_Y+((Mouse[NMouse].Radius*Seno[(int)Mouse[NMouse].Old_Dir]) >> 16), Color); |
||
465 | return OK; |
||
466 | } |
||
467 | |||
468 | TASK Redraw_Mouse_Task(int NMouse) |
||
469 | { |
||
470 | char str[40]; |
||
471 | TIME dwTicks; |
||
472 | while(1) |
||
473 | { |
||
474 | dwTicks=sys_gettime(NULL); |
||
475 | Draw_Mouse(NMouse, FALSE); |
||
476 | Draw_Mouse(NMouse, TRUE); |
||
477 | sprintf(str,"Colision: %4d", (int)Mouse[NMouse].NColision); |
||
478 | grx_text(str, NMouse*150, 21, 7, 0); |
||
479 | sprintf(str,"Motor: %4d Sensor 0: %4d Sensor 1: %4d Sensor 2: %4d Sensor 3: %4d", |
||
480 | (int)Mouse[NMouse].Motors[0].Old_V+(int)Mouse[NMouse].Motors[1].Old_V, |
||
481 | (int)Mouse[NMouse].Sensors[0].Value, (int)Mouse[NMouse].Sensors[1].Value, |
||
482 | (int)Mouse[NMouse].Sensors[2].Value, (int)Mouse[NMouse].Sensors[3].Value); |
||
483 | if (NMouse==0) |
||
484 | grx_text(str, 1, 1, 7, 0); |
||
485 | sprintf(str,"Sensor 4: %4d Sensor 5: %4d Ticks: %4d", |
||
486 | (int)Mouse[NMouse].Sensors[4].Value, (int)Mouse[NMouse].Sensors[5].Value, |
||
487 | (int)(sys_gettime(NULL)-dwTicks)/1000); |
||
488 | if (NMouse==0) |
||
489 | grx_text(str, 1, 11, 7, 0); |
||
490 | task_endcycle(); |
||
491 | } |
||
492 | } |
||
493 | |||
494 | TASK Redraw_Maze_Task(void) |
||
495 | { |
||
496 | while(1) |
||
497 | { |
||
498 | Draw_Maze(); |
||
499 | task_endcycle(); |
||
500 | } |
||
501 | } |
||
502 | |||
503 | TASK Referee(void) |
||
504 | { |
||
505 | int Counter, Box_Min_X, Box_Max_X ,Box_Min_Y, Box_Max_Y; |
||
506 | int deltaX, deltaY, Radius; |
||
507 | int Colision; |
||
508 | int NMouse; |
||
509 | |||
510 | while(1) |
||
511 | { |
||
512 | for(NMouse=0;NMouse<NMouses;NMouse++) |
||
513 | { |
||
514 | Colision=0; |
||
515 | Radius=Mouse[NMouse].Radius; |
||
516 | if(Mouse[NMouse].Y<(Maze.Y1+Radius)) |
||
517 | Colision++; |
||
518 | if(Mouse[NMouse].Y>(Maze.Y2-Radius)) |
||
519 | Colision++; |
||
520 | if(Mouse[NMouse].X<(Maze.X1+Radius)) |
||
521 | Colision++; |
||
522 | if(Mouse[NMouse].X>(Maze.X2-Radius)) |
||
523 | Colision++; |
||
524 | for(Counter=0; Counter<Maze.NBoxs; Counter++) |
||
525 | { |
||
526 | Box_Min_X=Maze.Boxs[Counter].X1-1; |
||
527 | Box_Max_X=Maze.Boxs[Counter].X2+1; |
||
528 | Box_Min_Y=Maze.Boxs[Counter].Y1-1; |
||
529 | Box_Max_Y=Maze.Boxs[Counter].Y2+1; |
||
530 | Radius=Mouse[NMouse].Radius; |
||
531 | if(Mouse[NMouse].Y<(Box_Min_Y-Radius)) |
||
532 | continue; |
||
533 | if(Mouse[NMouse].Y>(Box_Max_Y+Radius)) |
||
534 | continue; |
||
535 | if(Mouse[NMouse].X<(Box_Min_X-Radius)) |
||
536 | continue; |
||
537 | if(Mouse[NMouse].X>(Box_Max_X+Radius)) |
||
538 | continue; |
||
539 | if((Mouse[NMouse].X>=Box_Min_X) && (Mouse[NMouse].X<=Box_Max_X)) |
||
540 | if((Mouse[NMouse].Y>=(Box_Min_Y-Radius)) |
||
541 | && (Mouse[NMouse].Y<=(Box_Max_Y+Radius))) |
||
542 | Colision++; |
||
543 | if((Mouse[NMouse].Y>=Box_Min_Y) && (Mouse[NMouse].Y<=Box_Max_Y)) |
||
544 | if((Mouse[NMouse].X>=(Box_Min_X-Radius)) |
||
545 | && (Mouse[NMouse].X<=(Box_Max_X+Radius))) |
||
546 | Colision++; |
||
547 | Radius=sqr(Radius); |
||
548 | deltaX=sqr(Mouse[NMouse].X-Box_Min_X); |
||
549 | deltaY=sqr(Mouse[NMouse].Y-Box_Min_Y); |
||
550 | if(deltaX+deltaY<=Radius) |
||
551 | Colision++; |
||
552 | deltaY=sqr(Mouse[NMouse].Y-Box_Max_Y); |
||
553 | if(deltaX+deltaY<=Radius) |
||
554 | Colision++; |
||
555 | deltaX=sqr(Mouse[NMouse].X-Box_Max_X); |
||
556 | deltaY=sqr(Mouse[NMouse].Y-Box_Min_Y); |
||
557 | if(deltaX+deltaY<=Radius) |
||
558 | Colision++; |
||
559 | deltaY=sqr(Mouse[NMouse].Y-Box_Max_Y); |
||
560 | if(deltaX+deltaY<=Radius) |
||
561 | Colision++; |
||
562 | } |
||
563 | if (Mouse[NMouse].PColision && !Colision) |
||
564 | Mouse[NMouse].NColision++; |
||
565 | |||
566 | Mouse[NMouse].PColision=Colision; |
||
567 | } |
||
568 | task_endcycle(); |
||
569 | } |
||
570 | return (void *)0; |
||
571 | } |
||
572 | |||
573 | TASK Calculate_Position(int NMouse) |
||
574 | { |
||
575 | float w, v, dt; |
||
576 | int b; |
||
577 | while(1) |
||
578 | { |
||
579 | b=(Mouse[NMouse].Motors[MOTOR_LEFT].Radius- |
||
580 | Mouse[NMouse].Motors[MOTOR_RIGHT].Radius); |
||
581 | if (b<0) |
||
582 | b=-b; |
||
583 | w=(Mouse[NMouse].Motors[MOTOR_LEFT].Old_V- |
||
584 | Mouse[NMouse].Motors[MOTOR_RIGHT].Old_V); |
||
585 | w/=(float)b; |
||
586 | v=(Mouse[NMouse].Motors[MOTOR_LEFT].Old_V+ |
||
587 | Mouse[NMouse].Motors[MOTOR_RIGHT].Old_V); |
||
588 | v/=2.0; |
||
589 | dt=CALC_PERIOD/1000000.0; |
||
590 | Mouse[NMouse].Dir += (180.0*w/PI)*dt; |
||
591 | if (Mouse[NMouse].Dir >= 360) |
||
592 | Mouse[NMouse].Dir -= 360; |
||
593 | if (Mouse[NMouse].Dir < 0) |
||
594 | Mouse[NMouse].Dir += 360; |
||
595 | v *= dt/65536.0; |
||
596 | b=(float)Seno[(int)Mouse[NMouse].Dir+90]; |
||
597 | Mouse[NMouse].X += v*b; |
||
598 | b=(float)Seno[(int)Mouse[NMouse].Dir]; |
||
599 | Mouse[NMouse].Y += v*b; |
||
600 | Mouse[NMouse].Motors[0].Old_V= |
||
601 | (0.8187*Mouse[NMouse].Motors[0].Old_V+ |
||
602 | 0.1813*(float)Mouse[NMouse].Motors[0].V); |
||
603 | Mouse[NMouse].Motors[1].Old_V= |
||
604 | (0.8187*Mouse[NMouse].Motors[1].Old_V+ |
||
605 | 0.1813*(float)Mouse[NMouse].Motors[1].V); |
||
606 | task_endcycle(); |
||
607 | } |
||
608 | } |
||
609 | |||
610 | #define SENSOR_LEFT 0 |
||
611 | #define SENSOR_FRONT 1 |
||
612 | #define SENSOR_RIGTH 2 |
||
613 | #define SENSOR_GROUND 3 |
||
614 | #define SENSOR_B_LEFT 4 |
||
615 | #define SENSOR_B_RIGHT 5 |
||
616 | #define MAX_SPEED 50 |
||
617 | #define DIST_DIR GetSensor(NMouse, SENSOR_RIGTH) |
||
618 | #define DIST_ESQ GetSensor(NMouse, SENSOR_LEFT) |
||
619 | #define DIST_FRENTE GetSensor(NMouse, SENSOR_FRONT) |
||
620 | #define FAROL_ESQ GetSensor(NMouse, SENSOR_B_LEFT) |
||
621 | #define FAROL_DIR GetSensor(NMouse, SENSOR_B_RIGHT) |
||
622 | #define FAROL_LIM_INF SENSOR_MIN_BEACON+5 |
||
623 | |||
624 | #define DIST0 88 //Distancia minima optima |
||
625 | #define DIST1 DIST0 + 5 // 60% |
||
626 | #define DIST2 DIST1 + 10 // PARADO |
||
627 | #define DIST3 DIST2 + 10 // TRAS |
||
628 | |||
629 | #define DISTMAX 120 |
||
630 | #define NENHUM 0 |
||
631 | #define ESQUERDA 1 |
||
632 | #define DIREITA 2 |
||
633 | #define contorno ContornoArr[NMouse] |
||
634 | |||
635 | TASK MouseTask(int NMouse) |
||
636 | { |
||
637 | int i; |
||
638 | //char str[100]; |
||
639 | unsigned char flag; |
||
640 | static unsigned char ContornoArr[MAX_MOUSES]; |
||
641 | signed char modulo; |
||
642 | |||
643 | Init_Sensor(NMouse, SENSOR_LEFT, OBSTACLE, 14, -45, -45, 25, 15); // Left |
||
644 | Init_Sensor(NMouse, SENSOR_FRONT, OBSTACLE, 14, 0, 0, 25, 15); // Center |
||
645 | Init_Sensor(NMouse, SENSOR_RIGTH, OBSTACLE, 14, 45, 45, 25, 15); // Rigth |
||
646 | Init_Sensor(NMouse, SENSOR_GROUND, GROUND, 10, 180, 180, 0, 20); // Ground |
||
647 | Init_Sensor(NMouse, SENSOR_B_LEFT, BEACON, 4, -45, -25, 30, 0); // Left |
||
648 | Init_Sensor(NMouse, SENSOR_B_RIGHT, BEACON, 4, 45, 25, 30, 0); // Rigth |
||
649 | contorno = NENHUM; |
||
650 | Mouse[NMouse].Flag=1; |
||
651 | |||
652 | while(1) |
||
653 | { |
||
654 | for(i=0; i<8; i++) |
||
655 | Read_Sensor(NMouse, i); |
||
656 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED); |
||
657 | Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED); |
||
658 | |||
659 | flag = ((DIST_DIR>DIST3) && contorno==ESQUERDA) || |
||
660 | ((DIST_ESQ>DIST3) && contorno==DIREITA); |
||
661 | |||
662 | if ( (DIST_FRENTE>DISTMAX) || flag) { |
||
663 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED/2); |
||
664 | Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED/2); |
||
665 | if (contorno == ESQUERDA) |
||
666 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, -MAX_SPEED/2); |
||
667 | else |
||
668 | Set_Motor_Vel(NMouse, MOTOR_LEFT, -MAX_SPEED/2); |
||
669 | } |
||
670 | else { |
||
671 | |||
672 | // ---------------- Contorno da Esquerda --------------- |
||
673 | if (contorno!=DIREITA){ |
||
674 | if (DIST_ESQ>DIST1) { // Sensor da esquerda |
||
675 | contorno = ESQUERDA; |
||
676 | if (DIST_ESQ>DIST3) { |
||
677 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, -MAX_SPEED/2); |
||
678 | Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED/2); |
||
679 | } |
||
680 | else |
||
681 | if (DIST_ESQ>DIST2) |
||
682 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, 10); |
||
683 | else |
||
684 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED*3/4); |
||
685 | } |
||
686 | else |
||
687 | if (contorno== ESQUERDA) { |
||
688 | if(DIST_ESQ<DIST0) |
||
689 | Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED*3/10); |
||
690 | } |
||
691 | } |
||
692 | |||
693 | // ---------------- Contorno da Direita --------------- |
||
694 | if (contorno!=ESQUERDA){ |
||
695 | if (DIST_DIR>DIST1) { // Sensor da direita |
||
696 | contorno = DIREITA; |
||
697 | if (DIST_DIR>DIST3) { |
||
698 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED/2); |
||
699 | Set_Motor_Vel(NMouse, MOTOR_LEFT, -MAX_SPEED/2); |
||
700 | } |
||
701 | else |
||
702 | if (DIST_DIR>DIST2) |
||
703 | Set_Motor_Vel(NMouse, MOTOR_LEFT, 10); |
||
704 | else |
||
705 | Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED*3/4); |
||
706 | } |
||
707 | else |
||
708 | if (contorno== DIREITA) { |
||
709 | if (DIST_DIR<DIST0) |
||
710 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED*3/10); |
||
711 | } |
||
712 | } |
||
713 | |||
714 | // --------------- Desempancar contornos com o farol ------------ |
||
715 | |||
716 | if ((contorno == ESQUERDA && FAROL_DIR > FAROL_ESQ && FAROL_DIR > FAROL_LIM_INF) || |
||
717 | (contorno == DIREITA && FAROL_ESQ > FAROL_DIR && FAROL_ESQ > FAROL_LIM_INF) ) |
||
718 | contorno = NENHUM; |
||
719 | |||
720 | |||
721 | |||
722 | if (contorno == NENHUM) |
||
723 | { |
||
724 | modulo = FAROL_DIR - FAROL_ESQ; |
||
725 | if (modulo < 0 ) |
||
726 | modulo = - modulo; |
||
727 | |||
728 | // Se nao estiver na direēao certa, ajustar |
||
729 | if (modulo > 10) { |
||
730 | if (FAROL_DIR > FAROL_ESQ) |
||
731 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, 0); |
||
732 | else |
||
733 | Set_Motor_Vel(NMouse, MOTOR_LEFT, 0); |
||
734 | } |
||
735 | else //velocidade controlada |
||
736 | if (modulo > 2) { |
||
737 | if (FAROL_DIR > FAROL_ESQ) |
||
738 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED*6/10); |
||
739 | else |
||
740 | Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED*6/10); |
||
741 | } |
||
742 | } |
||
743 | } |
||
744 | if (GetSensor(NMouse, SENSOR_GROUND)==SENSOR_MAX_GROUND) |
||
745 | { |
||
746 | Set_Motor_Vel(NMouse, MOTOR_RIGHT, 0); |
||
747 | Set_Motor_Vel(NMouse, MOTOR_LEFT, 0); |
||
748 | } |
||
749 | |||
750 | task_endcycle(); |
||
751 | } |
||
752 | return (void *)0; |
||
753 | } |
||
754 | |||
755 | void my_end(void *arg) |
||
756 | { |
||
757 | grx_close(); |
||
758 | } |
||
759 | |||
760 | /* --------------------------------- Main program -----------------------------*/ |
||
761 | |||
762 | int main(int argc, char *argv[]) |
||
763 | { |
||
764 | int i; |
||
765 | char c; |
||
766 | int pid; |
||
767 | |||
768 | HARD_TASK_MODEL mp; |
||
769 | SOFT_TASK_MODEL ms; |
||
770 | |||
771 | // char StrAux[15]; |
||
772 | |||
773 | for (i=0; i<NMouses; i++) |
||
774 | Mouse[i].Flag=0; |
||
775 | for (i=0; i<(360+90); i++) // Inicializar a tabela de senos |
||
776 | Seno[i]=(int)(65536.0*sin((float)i/180.0*PI)); |
||
777 | |||
778 | for (i=0; i<180; i++) // Inicializar a tabela de tangente |
||
779 | if (i!=90) |
||
780 | Tang[i]=(int)(((float)Seno[i]/(float)Seno[i+90])*65536.0); |
||
781 | else |
||
782 | Tang[i]=INT_MAX; |
||
783 | |||
784 | // Definir funĘo de saida |
||
785 | sys_atrunlevel(my_end, (void *)NULL, RUNLEVEL_BEFORE_EXIT); |
||
786 | |||
787 | hard_task_default_model(mp); |
||
788 | hard_task_def_usemath(mp); |
||
789 | hard_task_def_group(mp, MOUSEGROUP); |
||
790 | hard_task_def_wcet(mp, 1); // wcet ignored by the configuration file! |
||
791 | |||
792 | soft_task_default_model(ms); |
||
793 | soft_task_def_group(ms, MOUSEGROUP); |
||
794 | soft_task_def_met(ms, 1000); |
||
795 | |||
796 | for (i=0;i<NMouses;i++) |
||
797 | { |
||
798 | hard_task_def_arg(mp, (void *)i); |
||
799 | soft_task_def_arg(ms, (void *)i); |
||
800 | |||
801 | hard_task_def_mit(mp, MOUSE_PERIOD); |
||
802 | pid = task_create("Mouse", MouseTask, &mp, NULL); |
||
803 | if (pid == NIL) |
||
804 | { |
||
805 | perror("Creating Mouse:"); |
||
806 | sys_end(); |
||
807 | } |
||
808 | |||
809 | hard_task_def_mit(mp, CALC_PERIOD); |
||
810 | pid = task_create("Calc", Calculate_Position, &mp, NULL); |
||
811 | if (pid == NIL) |
||
812 | { |
||
813 | perror("Creating Calc:"); |
||
814 | sys_end(); |
||
815 | } |
||
816 | |||
817 | soft_task_def_period(ms, REDRAW_MOUSE_PERIOD); |
||
818 | pid = task_create("Redraw_Mouse", Redraw_Mouse_Task, &ms, NULL); |
||
819 | if (pid == NIL) |
||
820 | { |
||
821 | perror("Creating Redraw_Moude:"); |
||
822 | sys_end(); |
||
823 | } |
||
824 | } |
||
825 | |||
826 | hard_task_def_mit(mp, REFEREE_PERIOD); |
||
827 | pid = task_create("Referee1", Referee, &mp, NULL); |
||
828 | |||
829 | hard_task_def_mit(mp, REDRAW_MAZE_PERIOD); |
||
830 | pid = task_create("Redraw_Maze", Redraw_Maze_Task, &mp, NULL); |
||
831 | |||
832 | grx_open(640,480,8); |
||
833 | group_activate(MOUSEGROUP); |
||
834 | do |
||
835 | { |
||
836 | c = keyb_getch(BLOCK); |
||
837 | } while (c != ESC); |
||
838 | |||
839 | task_nopreempt(); |
||
840 | sys_end(); |
||
841 | return 0; |
||
842 | } |
||
843 | |||
844 | /* the buffer b is scannedc to search for numbers |
||
845 | at the first non-number the function stops */ |
||
846 | void geti(char *b, int *pos, int *res) |
||
847 | { |
||
848 | // skip first chars |
||
849 | while (!isdigit(b[*pos])) |
||
850 | (*pos)++; |
||
851 | |||
852 | |||
853 | // read the numbers |
||
854 | *res = 0; |
||
855 | do { |
||
856 | *res = (*res * 10) + b[*pos] - '0'; |
||
857 | (*pos)++; |
||
858 | } while (isdigit(b[*pos])); |
||
859 | } |
||
860 | |||
861 | void read_cfg_file(int argc, char **argv) |
||
862 | { |
||
863 | int i, err, Temp1, Temp2, Temp3, Temp4, Temp5; |
||
864 | DOS_FILE *fp; |
||
865 | char myfilebuf[1000]; |
||
866 | int myfilebuf_length; |
||
867 | |||
868 | int pos = 0; |
||
869 | |||
870 | if (argc==2) { |
||
871 | fp = DOS_fopen(argv[1],"r"); |
||
872 | |||
873 | if (fp) { |
||
874 | /* read up to 1000 chars */ |
||
875 | myfilebuf_length = DOS_fread(&myfilebuf,1,1000,fp); |
||
876 | |||
877 | /* check for errors */ |
||
878 | err = DOS_error(); |
||
879 | |||
880 | cprintf("Read %d bytes...\n", myfilebuf_length); |
||
881 | |||
882 | if (err) { |
||
883 | cprintf("Error %d reading file...Using default values\n", err); |
||
884 | } |
||
885 | else |
||
886 | { |
||
887 | geti(myfilebuf, &pos, &NMouses); // Number of Mouses |
||
888 | for (i=0;i<NMouses;i++) { |
||
889 | geti(myfilebuf, &pos, &Temp1); // Mouse X coordinate |
||
890 | geti(myfilebuf, &pos, &Temp2); // Mouse Y coordinate |
||
891 | geti(myfilebuf, &pos, &Temp3); // Mouse radius |
||
892 | geti(myfilebuf, &pos, &Temp4); // Mouse orientation |
||
893 | Init_Mouse(i, Temp1, Temp2, Temp3, Temp4, 6); |
||
894 | } |
||
895 | geti(myfilebuf, &pos, &Temp1); // Beacon X coordinate |
||
896 | geti(myfilebuf, &pos, &Temp2); // Beacon Y coordinate |
||
897 | geti(myfilebuf, &pos, &Temp3); // Beacon intensity |
||
898 | geti(myfilebuf, &pos, &i); // Number of boxs |
||
899 | Init_Maze(Temp1, Temp2, Temp3, i); |
||
900 | for(; i>0; i--) { |
||
901 | geti(myfilebuf, &pos, &Temp1); // Box X1 Coordinate |
||
902 | geti(myfilebuf, &pos, &Temp2); // Box Y1 Coordinate |
||
903 | geti(myfilebuf, &pos, &Temp3); // Box X2 Coordinate |
||
904 | geti(myfilebuf, &pos, &Temp4); // Box Y2 Coordinate |
||
905 | geti(myfilebuf, &pos, &Temp5); // Type of Box |
||
906 | if (Temp5) |
||
907 | Init_Box(i-1, Temp1, Temp2, Temp3, Temp4, BOX_BLOCK); |
||
908 | else |
||
909 | Init_Box(i-1, Temp1, Temp2, Temp3, Temp4, BOX_NON_BLOCK); |
||
910 | } |
||
911 | DOS_fclose(fp); |
||
912 | return; |
||
913 | } |
||
914 | } |
||
915 | else { |
||
916 | /* error!! */ |
||
917 | err = DOS_error(); |
||
918 | /* note that if you call DOS_error() here, it return 0!!! */ |
||
919 | cprintf("Error %d opening myfile.txt...Using default values\n", err); |
||
920 | } |
||
921 | |||
922 | // Default value if no file is passed |
||
923 | NMouses=2; |
||
924 | Init_Maze(500, 350, 400, 1); |
||
925 | Init_Box(0, 200, 200, 300, 250, BOX_NON_BLOCK); |
||
926 | Init_Mouse(0, 100, 100, 15, 45, 6); |
||
927 | Init_Mouse(1, 100, 150, 15, 90, 6); |
||
928 | } |
||
929 | else { |
||
930 | cprintf("Wrong number of arguments...\n"); |
||
931 | l1_exit(0); |
||
932 | } |
||
933 | } |