Subversion Repositories shark

Rev

Blame | Last modification | View Log | RSS feed

/*--------------------------------------------------------------*/
/*              Title: Mouse                                    */
/*              Autor: João Capucho                             */
/*              Date: 9/12/2000                                 */
/*              Description:                                    */
/*              Simulation of small automats based on the       */
/*              "micro-rato" conteste                           */
/*--------------------------------------------------------------*/

/*
 * Copyright (C) 2001 João Capucho
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */


// Includes
#include <ll/i386/x-dos.h>
#include <kernel/kern.h>
#include <drivers/glib.h>
#include <drivers/keyb.h>
#include <math.h>
#include <stdlib.h>
#include <limits.h>

#define sqr(x)   (x*x)

// Error Types
#define OK                                      0
#define MEMORY_ERROR            -1
#define ILEGAL_PARAMETER        -2

// Types of sensors and motors
#define NONE                            0
#define OBSTACLE                        1
#define BEACON                          2
#define GROUND                          3
#define STEP                            4

// Maze limits
#define LAB_XMIN                        40     
#define LAB_XMAX                        600
#define LAB_YMIN                        40
#define LAB_YMAX                        440

#define MOTOR_RIGHT         1
#define MOTOR_LEFT          0

#define BACKCOLOR                       0       // Background color
#define OUTCOLOR            14   // Color of the outside walls
#define FINISHCOLOR         7   // Finishin area color
#define NONBLOCKCOLOR       14  // Non block boxs
#define BLOCKCOLOR          13  // Block boxs

#define BOX_BLOCK                       1       // The box blocks the beacon
#define BOX_NON_BLOCK           2       // The box doesn´t block the beacon
       
#define MAX_MOUSES                      5       // Maximum of mouses in the Maze
#define MAX_BOXS                        100 // Maximum of boxs in the Maze
#define MAX_SENSORS                     20      // Maximum of sensors in each mouse
#define SENSOR_MIN_GROUND       0       // Minimum value of a ground sensor
#define SENSOR_MAX_GROUND       255     // Maximum value of a ground sensor
#define SENSOR_MIN_OBSTACLE     77      // Minimum value of a obstacle sensor
#define SENSOR_MAX_OBSTACLE     128     // Maximum value of a obstacle sensor
#define SENSOR_MIN_BEACON       77      // Minimum value of a beacon sensor
#define SENSOR_MAX_BEACON       128     // Maximum value of a beacon sensor
#define BEACON_INTENSITY    400 // Maximun intensity of the beacon

#define MOUSE_PERIOD        20000    // Period
#define CALC_PERIOD         2000     // Period
#define REFEREE_PERIOD      1000     // Period of the referee
#define REDRAW_MOUSE_PERIOD 25000    // Period for redraw mouse
#define REDRAW_MAZE_PERIOD  100000   // Period for redraw maze
#define MOUSEGROUP          511

struct SENSOR
{
  int Type;                     // Tipe of sensor ( None - Obstacle - Beacon - Ground)
  int Radius;                   // Distance from de center of the mouse
  int Pos_Angle;                // Angle between the sensor and the front of the mouse
  int Dir;                      // Angle between the direction of the sensores and the front of the mouse
  int Angle;                    // Half angle of visual field (between (Dir-Angle) and (Dir+Angle))
  int Intensity;                // Sensativity of the sensor
  float Value;          // Last value readed
};

struct MOTOR
{
  int Radius;                   // Distance from de center of the mouse
  int V;              // Current velocity
  float Old_V;          // Previews velocity
};

struct MOUSE
{
  float X, Y;           // Current position of the center
  float Old_X, Old_Y;   // Previous position of the center
  int Radius;                   // Size of the mouse ( every mouse is considered round )
  float Dir;            // Points to the front
  float Old_Dir;        // Previous front
  int NSensors;         // Number of sensors in the mouse
  struct SENSOR Sensors[MAX_SENSORS];   // Pointer to the structer that contains the information of the sensors
  struct MOTOR Motors[2];               // Pointer to the structer that contains the information of the motors
  WORD    NColision;  // Total of colisions
  WORD    PColision;  // Auxiliar for colisions
  char    Flag;       // Initialization flag
};



struct BOX      // Structer that represents the obstacles
{                       //in this case all the obstacles are boxs
  int Type;                     // Type of the obstatcle
  int X1, Y1, X2, Y2;   // position of the box
};

struct SBEACON  // Beacon information
{
  int Intensity;        // Intensity of the beacon
  int X, Y;             // position of the beacon
};

struct MAZE
{
  int X1, Y1, X2, Y2;                   // limites of the maze
  int NBoxs;                                    // Number of objstacles in the maze
  struct BOX Boxs[MAX_BOXS];    // Information about all thr obstacles
  struct SBEACON Beacon;                // Beacon data
  int Radius;                 // Size of the finishing area
};

int     Seno[360+90];                   // Sine table
int     Tang[180];                              // Tangente table
int     NMouses=0;
struct  MOUSE   Mouse[MAX_MOUSES];              // Global variable that represents all the mouses
struct  MAZE    Maze;                                   // Global variable that represents the maze

// Init_Maze - Initialize the Maze
// Beacon_X - Coordinate X of the beacon
// Beacon_Y - Coordinate Y of the beacon
// Intensity - Intensity of the beacon
// NBoxs - Number of boxs in the maze
// Return:      OK - Sucess

int Init_Maze(int Beacon_X, int Beacon_Y, int Intensity, int NBoxs)
{
  int i;
  Maze.X1=LAB_XMIN;
  Maze.Y1=LAB_YMIN;
  Maze.X2=LAB_XMAX;
  Maze.Y2=LAB_YMAX;
  Maze.Beacon.X=Beacon_X+LAB_XMIN;
  Maze.Beacon.Y=Beacon_Y+LAB_YMIN;
  if (Intensity<BEACON_INTENSITY)
    Maze.Beacon.Intensity=Intensity;
  else
    Maze.Beacon.Intensity=BEACON_INTENSITY;
  if (NBoxs<MAX_BOXS)
    Maze.NBoxs=NBoxs;
  else
    Maze.NBoxs=MAX_BOXS;
  Maze.Radius=50;
  for(i=0; i < Maze.NBoxs; i++)
    Maze.Boxs[i].Type=NONE;
  return OK;
}

// Init_Box - Initialize data of the NBox obstacle
// NBox - Identifier of the obstacle
// X1 - Coordinate X
// Y1 - Coordinate Y
// X2 - Coordinate X
// Y2 - Coordinate Y
// Type - Type of the obstacle
// Return:      OK - Sucess

int Init_Box(int NBox, int X1, int Y1, int X2, int Y2, int Type)
{
  Maze.Boxs[NBox].Type=Type;
  if (X1<X2)
    {
      Maze.Boxs[NBox].X1=X1+LAB_XMIN;
      Maze.Boxs[NBox].X2=X2+LAB_XMIN;
    }
  else
    {
      Maze.Boxs[NBox].X1=X2+LAB_XMIN;
      Maze.Boxs[NBox].X2=X1+LAB_XMIN;
    }
  if (Y1<Y2)
    {
      Maze.Boxs[NBox].Y1=Y1+LAB_YMIN;
      Maze.Boxs[NBox].Y2=Y2+LAB_YMIN;
    }
  else
    {
      Maze.Boxs[NBox].Y1=Y2+LAB_YMIN;
      Maze.Boxs[NBox].Y2=Y1+LAB_YMIN;
    }
  return OK;
}

// Draw_Maze - Draws the outline of the maze and the obstacles
// Return:      OK - Sucess
int Draw_Maze(void)
{
  int i;
  grx_rect( Maze.X1, Maze.Y1, Maze.X2, Maze.Y2, OUTCOLOR);
  grx_rect( Maze.X1-1, Maze.Y1+1, Maze.X2+1, Maze.Y2+1, OUTCOLOR);
  grx_rect( Maze.X1-2, Maze.Y1+2, Maze.X2+2, Maze.Y2+2, OUTCOLOR);
  grx_circle(Maze.Beacon.X, Maze.Beacon.Y, Maze.Radius, FINISHCOLOR);
  grx_disc(Maze.Beacon.X, Maze.Beacon.Y, 10, 7);
  for (i=0; i<Maze.NBoxs; i++)
    {
      if (Maze.Boxs[i].Type==BOX_NON_BLOCK)
        grx_box(Maze.Boxs[i].X1, Maze.Boxs[i].Y1,
                Maze.Boxs[i].X2, Maze.Boxs[i].Y2, NONBLOCKCOLOR);
      if (Maze.Boxs[i].Type==BOX_BLOCK)
        grx_box(Maze.Boxs[i].X1, Maze.Boxs[i].Y1,
                Maze.Boxs[i].X2, Maze.Boxs[i].Y2, BLOCKCOLOR);
    }
  return OK;
}

int Init_Motor(int NMouse, int Motor, int Radius)
{
  if ((NMouse<0) || (NMouse>=MAX_MOUSES))
    return ILEGAL_PARAMETER;           
  if ((Motor<0) || (Motor>=2))
    return ILEGAL_PARAMETER;           
  Mouse[NMouse].Motors[Motor].Radius=Radius;
  Mouse[NMouse].Motors[Motor].V=0;
  Mouse[NMouse].Motors[Motor].Old_V=0.0;
  return OK;
}

// Init_Mouse - inicialize the parameters of each mouse
// NMouse - Identifier of the mouse
// X - Coordinate X of the center
// Y - Coordinate Y of the center
// Radius - Radius of the mouse (size)
// Dir - Angle of direction in witch i will start
// NSensors - Number of sensors in the mouse (all types)
//
// Return:      OK - Sucess
//                      ILEGAL_PARAMETER - If NMouse negative of greater than maximum number of mouses
// Note: All the types of sensors and motors are initialize to NONE

int Init_Mouse(int NMouse, int X, int Y, int Radius, int Dir, int NSensors)
{
  int i;
  if ((NMouse<0) || (NMouse>=MAX_MOUSES))
    return ILEGAL_PARAMETER;
  if ((NSensors<0) || (NSensors>MAX_SENSORS))
    return ILEGAL_PARAMETER;
  Mouse[NMouse].X=(float)X+LAB_XMIN;
  Mouse[NMouse].Y=(float)Y+LAB_YMIN;
  Mouse[NMouse].Radius=Radius;
  Mouse[NMouse].Dir=Dir;
  Mouse[NMouse].Old_Dir=Mouse[NMouse].Dir;
  Mouse[NMouse].NSensors=NSensors;
  Mouse[NMouse].Old_X=Mouse[NMouse].X;
  Mouse[NMouse].Old_Y=Mouse[NMouse].Y;
  Mouse[NMouse].NColision=0;
  Mouse[NMouse].PColision=0;
  for(i=0; i < Mouse[NMouse].NSensors; i++)
    Mouse[NMouse].Sensors[i].Type=NONE;

  Init_Motor(NMouse, MOTOR_LEFT, 10);
  Init_Motor(NMouse, MOTOR_RIGHT, -10);
       
  return OK;
}

// Init_Sensor - Initializes the sensor information
// NMouse - Identifier of the mouse
// Sensor - Identifier of the sensor to be used
// Type - Type of sensor ( Ground, obstacle or beacon sensor )
// Radius and Pos_Angle - Polar coordinates to determen the sensor position
// Dir - Diretion of the sensor ( angle between the front and the sensor )
// Angle - Half of the visual range
// Intensity - Maximun distance of detect
int Init_Sensor(int NMouse, int Sensor, int Type, int Radius,
                int Pos_Angle, int Dir, int Angle, int Intensity)
{
  if ((NMouse<0) || (NMouse>=MAX_MOUSES))
    return ILEGAL_PARAMETER;           
  if ((Sensor<0) || (Sensor>=Mouse[NMouse].NSensors))
    return ILEGAL_PARAMETER;           
  Mouse[NMouse].Sensors[Sensor].Type=Type;
  Mouse[NMouse].Sensors[Sensor].Radius=Radius;
  Mouse[NMouse].Sensors[Sensor].Pos_Angle=Pos_Angle;
  Mouse[NMouse].Sensors[Sensor].Dir=Dir;
  Mouse[NMouse].Sensors[Sensor].Angle=Angle;
  Mouse[NMouse].Sensors[Sensor].Intensity=Intensity;
  if (Type == GROUND)
    Mouse[NMouse].Sensors[Sensor].Value=(float)SENSOR_MIN_GROUND;
  if (Type == OBSTACLE)
    Mouse[NMouse].Sensors[Sensor].Value=(float)SENSOR_MIN_OBSTACLE;
  if (Type == BEACON)
    Mouse[NMouse].Sensors[Sensor].Value=(float)SENSOR_MIN_BEACON;
  return OK;
}

int Set_Motor_Vel(int NMouse, int NMotor, int Vel)
{
  if ((NMouse<0) || (NMouse>=MAX_MOUSES))
    return ILEGAL_PARAMETER;
  if ((NMotor<0) || (NMotor>2))
    return ILEGAL_PARAMETER;           
  Mouse[NMouse].Motors[NMotor].V=Vel;
  return OK;
}

int Read_Sensor(int NMouse, int NSensor)
{
  int X, Y, Dir, cr, sr, AuxX, AuxY;
  int Count, AuxCount, AngCount, AuxAngCount, RetColor, Intensity;    
  float Auxf;

  /* Inicial verifications */
  if ((NMouse<0) || (NMouse>=MAX_MOUSES))
    return ILEGAL_PARAMETER;
  if ((NSensor<0) || (NSensor>Mouse[NMouse].NSensors))
    return ILEGAL_PARAMETER;           
       
  /* If Sensor Not define exit */
  if ( Mouse[NMouse].Sensors[NSensor].Type == NONE )
    return OK;
       
  /* Calculate de position of the sensor and is direction */
  Dir=(int)Mouse[NMouse].Old_Dir+Mouse[NMouse].Sensors[NSensor].Dir;
  if (Dir >= 360)
    Dir -= 360;
  if (Dir < 0)
    Dir += 360;
  X= Mouse[NMouse].Old_X*65536.0+((Mouse[NMouse].Sensors[NSensor].Radius*Seno[Dir+90]));
  Y= Mouse[NMouse].Old_Y*65536.0+((Mouse[NMouse].Sensors[NSensor].Radius*Seno[Dir]));

  /* if the Sensor type is OBSTACLE */
  if ( Mouse[NMouse].Sensors[NSensor].Type == OBSTACLE )
    {
      Intensity=Mouse[NMouse].Sensors[NSensor].Intensity;
      AuxCount=Intensity;
      /* Scan all the visual field */
      for (AngCount=(-Mouse[NMouse].Sensors[NSensor].Angle);
           AngCount<=Mouse[NMouse].Sensors[NSensor].Angle;
           AngCount++)
        {
          AuxAngCount=Dir+AngCount;
          if (AuxAngCount >= 360)
            AuxAngCount -= 360;
          if (AuxAngCount < 0)
            AuxAngCount += 360;            
          sr=Seno[AuxAngCount];
          cr=Seno[AuxAngCount+90];
          Count=Mouse[NMouse].Radius-Mouse[NMouse].Sensors[NSensor].Radius+2;
          AuxX = X+cr*Count;
          AuxY = Y+sr*Count;
          for (; Count<=AuxCount; Count++)
            {
              AuxX += cr;
              AuxY += sr;
              RetColor = grx_getpixel(AuxX >> 16, AuxY >> 16);
              if ((RetColor != BACKCOLOR)&&(RetColor != FINISHCOLOR))
                AuxCount=Count;
            }
        }
      Count=Mouse[NMouse].Sensors[NSensor].Intensity;
      Count*=Count;
      AuxCount*=AuxCount;
      Count = SENSOR_MAX_OBSTACLE-((SENSOR_MAX_OBSTACLE-SENSOR_MIN_OBSTACLE)
                                   *AuxCount)/Count;
      Mouse[NMouse].Sensors[NSensor].Value=
        (0.5*Mouse[NMouse].Sensors[NSensor].Value+0.5*(float)Count);
    }
  if ( Mouse[NMouse].Sensors[NSensor].Type == GROUND )
    {
      Mouse[NMouse].Sensors[NSensor].Value=(float)SENSOR_MIN_GROUND;
      AuxX=abs((X>>16)-Maze.Beacon.X);
      AuxY=abs((Y>>16)-Maze.Beacon.Y);
      if ((AuxX>50) || (AuxY>50))
        return OK;
      if ((AuxX*AuxX+AuxY*AuxY)<Maze.Radius*Maze.Radius)
        Mouse[NMouse].Sensors[NSensor].Value=(float)SENSOR_MAX_GROUND;  
    }

  if ( Mouse[NMouse].Sensors[NSensor].Type == BEACON )
    {
      AuxX=(X>>16)-Maze.Beacon.X;
      AuxY=(Y>>16)-Maze.Beacon.Y;
      AngCount=0;
      if (AuxX!=0)
        {
          AuxAngCount=(AuxY<<16)/AuxX;
          if (AuxAngCount<0)
            AngCount=90;
          for (Count=AngCount;Count<AngCount+90;Count++)
            {
              if ((Tang[Count]<=AuxAngCount) && (Tang[Count+1]>AuxAngCount))
                break;
            }
        }
      else
        Count=90;
      if (AuxY<0)      
        Count+=180;
      AuxAngCount=abs(180-abs(Dir-Count));
      Count=SENSOR_MIN_BEACON;
      if (AuxAngCount<Mouse[NMouse].Sensors[NSensor].Angle)
        {
          Count=AuxX*AuxX+AuxY*AuxY;
          AuxCount=(int)((float)Maze.Beacon.Intensity*(1.0+(0.4*(float)AuxAngCount)/(float)Mouse[NMouse].Sensors[NSensor].Angle));
          AuxCount*=AuxCount;
          if (Count<AuxCount)
            Count = SENSOR_MAX_OBSTACLE-((SENSOR_MAX_OBSTACLE-SENSOR_MIN_OBSTACLE)*Count)/AuxCount;
          else
            Count=SENSOR_MIN_BEACON;
        }
      Auxf=0.5*((float)Count);
      Auxf+=0.5*((float)Mouse[NMouse].Sensors[NSensor].Value);
      Mouse[NMouse].Sensors[NSensor].Value=Auxf;
    }
  return OK;
}

int GetSensor(int NMouse, int NSensor)
{
  if ((NMouse<0) || (NMouse>=MAX_MOUSES))
    return ILEGAL_PARAMETER;
  if ((NSensor<0) || (NSensor>Mouse[NMouse].NSensors))
    return ILEGAL_PARAMETER;           
  return (int)Mouse[NMouse].Sensors[NSensor].Value;    
}

int Draw_Mouse(int NMouse, int Flag)
{
  int Color;
  if (!Flag)
    Color=BACKCOLOR;
  else
    {
      Mouse[NMouse].Old_X=Mouse[NMouse].X;
      Mouse[NMouse].Old_Y=Mouse[NMouse].Y;
      Mouse[NMouse].Old_Dir=Mouse[NMouse].Dir;
      Color=2;
    }
  grx_circle(Mouse[NMouse].Old_X, Mouse[NMouse].Old_Y, Mouse[NMouse].Radius, Color);
  grx_line(Mouse[NMouse].Old_X, Mouse[NMouse].Old_Y,
           Mouse[NMouse].Old_X+((Mouse[NMouse].Radius*Seno[(int)Mouse[NMouse].Old_Dir+90]) >> 16),
           Mouse[NMouse].Old_Y+((Mouse[NMouse].Radius*Seno[(int)Mouse[NMouse].Old_Dir]) >> 16), Color);        
  return OK;
}

TASK Redraw_Mouse_Task(int NMouse)
{
  char str[40];
  TIME dwTicks;
  while(1)
    {
      dwTicks=sys_gettime(NULL);
      Draw_Mouse(NMouse, FALSE);
      Draw_Mouse(NMouse, TRUE);
      sprintf(str,"Colision: %4d", (int)Mouse[NMouse].NColision);
      grx_text(str, NMouse*150, 21, 7, 0);
      sprintf(str,"Motor: %4d  Sensor 0: %4d  Sensor 1: %4d  Sensor 2: %4d Sensor 3: %4d",
              (int)Mouse[NMouse].Motors[0].Old_V+(int)Mouse[NMouse].Motors[1].Old_V,
              (int)Mouse[NMouse].Sensors[0].Value, (int)Mouse[NMouse].Sensors[1].Value,
              (int)Mouse[NMouse].Sensors[2].Value, (int)Mouse[NMouse].Sensors[3].Value);
      if (NMouse==0)
        grx_text(str, 1, 1, 7, 0);
      sprintf(str,"Sensor 4: %4d  Sensor 5: %4d  Ticks: %4d",
              (int)Mouse[NMouse].Sensors[4].Value, (int)Mouse[NMouse].Sensors[5].Value,          
              (int)(sys_gettime(NULL)-dwTicks)/1000);
      if (NMouse==0)
        grx_text(str, 1, 11, 7, 0);
      task_endcycle();
    }  
}

TASK Redraw_Maze_Task(void)
{
  while(1)
    {
      Draw_Maze();
      task_endcycle();
    }  
}

TASK Referee(void)
{
  int Counter, Box_Min_X, Box_Max_X ,Box_Min_Y, Box_Max_Y;
  int deltaX, deltaY, Radius;
  int Colision;
  int NMouse;
 
  while(1)
    {
      for(NMouse=0;NMouse<NMouses;NMouse++)
        {
          Colision=0;
          Radius=Mouse[NMouse].Radius;
          if(Mouse[NMouse].Y<(Maze.Y1+Radius))
            Colision++;
          if(Mouse[NMouse].Y>(Maze.Y2-Radius))
            Colision++;
          if(Mouse[NMouse].X<(Maze.X1+Radius))
            Colision++;
          if(Mouse[NMouse].X>(Maze.X2-Radius))
            Colision++;
          for(Counter=0; Counter<Maze.NBoxs; Counter++)
            {
              Box_Min_X=Maze.Boxs[Counter].X1-1;
              Box_Max_X=Maze.Boxs[Counter].X2+1;
              Box_Min_Y=Maze.Boxs[Counter].Y1-1;
              Box_Max_Y=Maze.Boxs[Counter].Y2+1;
              Radius=Mouse[NMouse].Radius;
              if(Mouse[NMouse].Y<(Box_Min_Y-Radius))
                continue;
              if(Mouse[NMouse].Y>(Box_Max_Y+Radius))
                continue;
              if(Mouse[NMouse].X<(Box_Min_X-Radius))
                continue;
              if(Mouse[NMouse].X>(Box_Max_X+Radius))
                continue;
              if((Mouse[NMouse].X>=Box_Min_X) && (Mouse[NMouse].X<=Box_Max_X))
                if((Mouse[NMouse].Y>=(Box_Min_Y-Radius))
                   && (Mouse[NMouse].Y<=(Box_Max_Y+Radius)))
                  Colision++;
              if((Mouse[NMouse].Y>=Box_Min_Y) && (Mouse[NMouse].Y<=Box_Max_Y))
                if((Mouse[NMouse].X>=(Box_Min_X-Radius))
                   && (Mouse[NMouse].X<=(Box_Max_X+Radius)))
                  Colision++;
              Radius=sqr(Radius);
              deltaX=sqr(Mouse[NMouse].X-Box_Min_X);
              deltaY=sqr(Mouse[NMouse].Y-Box_Min_Y);
              if(deltaX+deltaY<=Radius)
                Colision++;
              deltaY=sqr(Mouse[NMouse].Y-Box_Max_Y);
              if(deltaX+deltaY<=Radius)
                Colision++;
              deltaX=sqr(Mouse[NMouse].X-Box_Max_X);
              deltaY=sqr(Mouse[NMouse].Y-Box_Min_Y);
              if(deltaX+deltaY<=Radius)
                Colision++;
              deltaY=sqr(Mouse[NMouse].Y-Box_Max_Y);
              if(deltaX+deltaY<=Radius)
                Colision++;
            }
          if (Mouse[NMouse].PColision && !Colision)
            Mouse[NMouse].NColision++;

          Mouse[NMouse].PColision=Colision;
        }
      task_endcycle();
    }
    return (void *)0;
}

TASK Calculate_Position(int NMouse)
{
  float w, v, dt;
  int b;
  while(1)
    {
      b=(Mouse[NMouse].Motors[MOTOR_LEFT].Radius-
         Mouse[NMouse].Motors[MOTOR_RIGHT].Radius);
      if (b<0)
        b=-b;
      w=(Mouse[NMouse].Motors[MOTOR_LEFT].Old_V-
         Mouse[NMouse].Motors[MOTOR_RIGHT].Old_V);
      w/=(float)b;
      v=(Mouse[NMouse].Motors[MOTOR_LEFT].Old_V+
         Mouse[NMouse].Motors[MOTOR_RIGHT].Old_V);
      v/=2.0;
      dt=CALC_PERIOD/1000000.0;
      Mouse[NMouse].Dir += (180.0*w/PI)*dt;
      if (Mouse[NMouse].Dir >= 360)
        Mouse[NMouse].Dir -= 360;
      if (Mouse[NMouse].Dir < 0)
        Mouse[NMouse].Dir += 360;
      v *= dt/65536.0;
      b=(float)Seno[(int)Mouse[NMouse].Dir+90];
      Mouse[NMouse].X += v*b;
      b=(float)Seno[(int)Mouse[NMouse].Dir];
      Mouse[NMouse].Y += v*b;
      Mouse[NMouse].Motors[0].Old_V=
        (0.8187*Mouse[NMouse].Motors[0].Old_V+
         0.1813*(float)Mouse[NMouse].Motors[0].V);
      Mouse[NMouse].Motors[1].Old_V=
        (0.8187*Mouse[NMouse].Motors[1].Old_V+
         0.1813*(float)Mouse[NMouse].Motors[1].V);
      task_endcycle();
    }
}

#define SENSOR_LEFT     0
#define SENSOR_FRONT    1
#define SENSOR_RIGTH    2
#define SENSOR_GROUND   3
#define SENSOR_B_LEFT   4
#define SENSOR_B_RIGHT  5
#define MAX_SPEED       50
#define DIST_DIR        GetSensor(NMouse, SENSOR_RIGTH)
#define DIST_ESQ        GetSensor(NMouse, SENSOR_LEFT)
#define DIST_FRENTE     GetSensor(NMouse, SENSOR_FRONT)
#define FAROL_ESQ       GetSensor(NMouse, SENSOR_B_LEFT)
#define FAROL_DIR       GetSensor(NMouse, SENSOR_B_RIGHT)
#define FAROL_LIM_INF   SENSOR_MIN_BEACON+5

#define DIST0 88        //Distancia minima optima
#define DIST1 DIST0 + 5  // 60%
#define DIST2 DIST1 + 10  // PARADO
#define DIST3 DIST2 + 10  // TRAS

#define DISTMAX 120
#define NENHUM   0
#define ESQUERDA 1
#define DIREITA  2
#define contorno ContornoArr[NMouse]

TASK MouseTask(int NMouse)
{
  int i;
  //char str[100];
  unsigned char flag;
  static unsigned char ContornoArr[MAX_MOUSES];
  signed char modulo;
         
  Init_Sensor(NMouse, SENSOR_LEFT, OBSTACLE, 14, -45, -45, 25, 15);   // Left
  Init_Sensor(NMouse, SENSOR_FRONT, OBSTACLE, 14, 0, 0, 25, 15);  // Center
  Init_Sensor(NMouse, SENSOR_RIGTH, OBSTACLE, 14, 45, 45, 25, 15);    // Rigth
  Init_Sensor(NMouse, SENSOR_GROUND, GROUND, 10, 180, 180, 0, 20);      // Ground
  Init_Sensor(NMouse, SENSOR_B_LEFT, BEACON, 4, -45, -25, 30, 0); // Left
  Init_Sensor(NMouse, SENSOR_B_RIGHT, BEACON, 4, 45, 25, 30, 0);  // Rigth
  contorno = NENHUM;
  Mouse[NMouse].Flag=1;

  while(1)
    {
      for(i=0; i<8; i++)
        Read_Sensor(NMouse, i);
      Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED);
      Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED);
 
      flag = ((DIST_DIR>DIST3) && contorno==ESQUERDA) ||
        ((DIST_ESQ>DIST3) && contorno==DIREITA);  

      if ( (DIST_FRENTE>DISTMAX) || flag) {
        Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED/2);
        Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED/2);
        if (contorno == ESQUERDA)
          Set_Motor_Vel(NMouse, MOTOR_RIGHT, -MAX_SPEED/2);
        else
          Set_Motor_Vel(NMouse, MOTOR_LEFT, -MAX_SPEED/2);
      }
      else {

        // ---------------- Contorno da Esquerda ---------------
        if (contorno!=DIREITA){
          if (DIST_ESQ>DIST1) {   // Sensor da esquerda
            contorno = ESQUERDA;
            if (DIST_ESQ>DIST3) {
              Set_Motor_Vel(NMouse, MOTOR_RIGHT, -MAX_SPEED/2);
              Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED/2);
            }
            else
              if (DIST_ESQ>DIST2)
                Set_Motor_Vel(NMouse, MOTOR_RIGHT, 10);
              else
                Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED*3/4);
          }                
          else              
            if (contorno== ESQUERDA) {
              if(DIST_ESQ<DIST0)
                Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED*3/10);
            }
        }
 
        // ---------------- Contorno da Direita ---------------
        if (contorno!=ESQUERDA){
          if (DIST_DIR>DIST1) {   // Sensor da direita
            contorno = DIREITA;
            if (DIST_DIR>DIST3) {
              Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED/2);
              Set_Motor_Vel(NMouse, MOTOR_LEFT, -MAX_SPEED/2);
            }
            else
              if (DIST_DIR>DIST2)
                Set_Motor_Vel(NMouse, MOTOR_LEFT, 10);
              else
                Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED*3/4);
          }                
          else              
            if (contorno== DIREITA) {
              if (DIST_DIR<DIST0)
                Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED*3/10);
            }
        }

        // --------------- Desempancar contornos com o  farol ------------

        if ((contorno == ESQUERDA && FAROL_DIR > FAROL_ESQ && FAROL_DIR > FAROL_LIM_INF) ||
            (contorno == DIREITA  && FAROL_ESQ > FAROL_DIR && FAROL_ESQ > FAROL_LIM_INF) )
          contorno = NENHUM;


       
        if (contorno == NENHUM)    
          {      
            modulo = FAROL_DIR - FAROL_ESQ;
            if (modulo < 0 )
              modulo = - modulo;
       
            // Se nao estiver na direçao certa, ajustar
            if (modulo > 10) {
              if (FAROL_DIR > FAROL_ESQ)
                Set_Motor_Vel(NMouse, MOTOR_RIGHT, 0);
              else
                Set_Motor_Vel(NMouse, MOTOR_LEFT, 0);
            }
            else //velocidade controlada
              if (modulo > 2) {
                if (FAROL_DIR > FAROL_ESQ)
                  Set_Motor_Vel(NMouse, MOTOR_RIGHT, MAX_SPEED*6/10);
                else
                  Set_Motor_Vel(NMouse, MOTOR_LEFT, MAX_SPEED*6/10);
              }
          }
      }
      if (GetSensor(NMouse, SENSOR_GROUND)==SENSOR_MAX_GROUND)
        {
          Set_Motor_Vel(NMouse, MOTOR_RIGHT, 0);
          Set_Motor_Vel(NMouse, MOTOR_LEFT, 0);
        }

      task_endcycle();
    }
    return (void *)0;
}

void my_end(void *arg)
{
  grx_close();
}

/* --------------------------------- Main program -----------------------------*/

int main(int argc, char *argv[])
{
  int i;
  char c;
  int pid;

  HARD_TASK_MODEL mp;
  SOFT_TASK_MODEL ms;

  //  char StrAux[15];

  set_exchandler_grx();

  for (i=0; i<NMouses; i++)
    Mouse[i].Flag=0;
  for (i=0; i<(360+90); i++)             // Inicializar a tabela de senos
    Seno[i]=(int)(65536.0*sin((float)i/180.0*PI));

  for (i=0; i<180; i++)                         // Inicializar a tabela de tangente
    if (i!=90)
      Tang[i]=(int)(((float)Seno[i]/(float)Seno[i+90])*65536.0);
    else
      Tang[i]=INT_MAX;
             
  // Definir fun‡Æo de saida
  sys_atrunlevel(my_end, (void *)NULL, RUNLEVEL_BEFORE_EXIT);

  hard_task_default_model(mp);
  hard_task_def_usemath(mp);
  hard_task_def_group(mp, MOUSEGROUP);
  hard_task_def_wcet(mp, 1); // wcet ignored by the configuration file!

  soft_task_default_model(ms);
  soft_task_def_group(ms, MOUSEGROUP);
  soft_task_def_met(ms, 1000);

  for (i=0;i<NMouses;i++)
    {
      hard_task_def_arg(mp, (void *)i);
      soft_task_def_arg(ms, (void *)i);

      hard_task_def_mit(mp, MOUSE_PERIOD);
      pid = task_create("Mouse", MouseTask, &mp, NULL);
      if (pid == NIL)
        {
          perror("Creating Mouse:");
          sys_end();
        }

      hard_task_def_mit(mp, CALC_PERIOD);
      pid = task_create("Calc", Calculate_Position, &mp, NULL);
      if (pid == NIL)
        {
          perror("Creating Calc:");
          sys_end();
        }

      soft_task_def_period(ms, REDRAW_MOUSE_PERIOD);
      pid = task_create("Redraw_Mouse", Redraw_Mouse_Task, &ms, NULL);
      if (pid == NIL)
        {
          perror("Creating Redraw_Moude:");
          sys_end();
        }
    }

  hard_task_def_mit(mp, REFEREE_PERIOD);
  pid = task_create("Referee1", Referee, &mp, NULL);

  hard_task_def_mit(mp, REDRAW_MAZE_PERIOD);
  pid = task_create("Redraw_Maze", Redraw_Maze_Task, &mp, NULL);

  grx_open(640,480,8);
  group_activate(MOUSEGROUP);
  do
    {
      c = keyb_getch(BLOCK);
    } while (c != ESC);

  task_nopreempt();
  sys_end();
  return 0;
}

/* the buffer b is scannedc to search for numbers
   at the first non-number the function stops */
void geti(char *b, int *pos, int *res)
{
  // skip first chars
  while (!isdigit(b[*pos]))
    (*pos)++;


  // read the numbers
  *res = 0;
  do {
    *res = (*res * 10) + b[*pos] - '0';
    (*pos)++;
  } while (isdigit(b[*pos]));
}

void read_cfg_file(int argc, char **argv)
{
  int i, err, Temp1, Temp2, Temp3, Temp4, Temp5;
  DOS_FILE      *fp;
  char myfilebuf[1000];
  int myfilebuf_length;

  int pos = 0;
   
  if (argc==2) {
    fp = DOS_fopen(argv[1],"r");

    if (fp) {
      /* read up to 1000 chars */
      myfilebuf_length = DOS_fread(&myfilebuf,1,1000,fp);
   
      /* check for errors */
      err = DOS_error();
   
      cprintf("Read %d bytes...\n", myfilebuf_length);
   
      if (err) {
        cprintf("Error %d reading file...Using default values\n", err);
      }
      else
      {
        geti(myfilebuf, &pos, &NMouses);          // Number of Mouses
        for (i=0;i<NMouses;i++) {
          geti(myfilebuf, &pos, &Temp1);  // Mouse X coordinate
          geti(myfilebuf, &pos, &Temp2);  // Mouse Y coordinate
          geti(myfilebuf, &pos, &Temp3);  // Mouse radius
          geti(myfilebuf, &pos, &Temp4);  // Mouse orientation
          Init_Mouse(i, Temp1, Temp2, Temp3, Temp4, 6);
        }
        geti(myfilebuf, &pos, &Temp1);      // Beacon X coordinate
        geti(myfilebuf, &pos, &Temp2);      // Beacon Y coordinate
        geti(myfilebuf, &pos, &Temp3);      // Beacon intensity
        geti(myfilebuf, &pos, &i);          // Number of boxs
        Init_Maze(Temp1, Temp2, Temp3, i);
        for(; i>0; i--) {
          geti(myfilebuf, &pos, &Temp1);  // Box X1 Coordinate
          geti(myfilebuf, &pos, &Temp2);  // Box Y1 Coordinate
          geti(myfilebuf, &pos, &Temp3);  // Box X2 Coordinate
          geti(myfilebuf, &pos, &Temp4);  // Box Y2 Coordinate
          geti(myfilebuf, &pos, &Temp5); // Type of Box
          if (Temp5)
            Init_Box(i-1, Temp1, Temp2, Temp3, Temp4, BOX_BLOCK);
          else
            Init_Box(i-1, Temp1, Temp2, Temp3, Temp4, BOX_NON_BLOCK);
        }
        DOS_fclose(fp);
        return;
      }
    }
    else {
      /* error!! */
      err = DOS_error();
      /* note that if you call DOS_error() here, it return 0!!! */
      cprintf("Error %d opening myfile.txt...Using default values\n", err);
    }

    // Default value if no file is passed
    NMouses=2;
    Init_Maze(500, 350, 400, 1);
    Init_Box(0, 200, 200, 300, 250, BOX_NON_BLOCK);
    Init_Mouse(0, 100, 100, 15, 45, 6);
    Init_Mouse(1, 100, 150, 15, 90, 6);
  }
  else {
    cprintf("Wrong number of arguments...\n");
    l1_exit(0);
  }
}