Subversion Repositories shark

/*--------------------------------------------------------------*/

/*              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);