Subversion Repositories shark

Rev

Rev 1086 | 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
}