Rev 1086 |
Go to most recent revision |
Blame |
Compare with Previous |
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];
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);
}
}