/unsupported/tags/rel_0_5/joy/makefile |
File deleted |
|
Property changes: |
Deleted: svn:executable |
## -1 +0,0 ## |
-* |
\ No newline at end of property |
Index: rel_0_5/joy/joy_test.c |
=================================================================== |
--- rel_0_5/joy/joy_test.c (revision 1657) |
+++ rel_0_5/joy/joy_test.c (nonexistent) |
@@ -1,131 +0,0 @@ |
- |
-/* |
- * Project: S.Ha.R.K. |
- * |
- * Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
- * |
- * Authors : Paolo Gai <pj@hartik.sssup.it> |
- * (see authors.txt for full list of hartik's authors) |
- * |
- * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
- * |
- * http://www.sssup.it |
- * http://retis.sssup.it |
- * http://shark.sssup.it |
- */ |
- |
-/* |
- * Copyright (C) 2000 Paolo Gai |
- * |
- * 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 |
- * |
- */ |
- |
-#include <kernel/kern.h> |
-#include <kernel/func.h> |
-#include <stdlib.h> |
-#include <drivers/keyb.h> |
-#include <math.h> |
- |
-#include "joy.h" |
- |
-PID pid; |
- |
-void endfun(KEY_EVT *k) |
-{ |
- cprintf("Ctrl-Brk pressed! Ending...\n"); |
- sys_end(); |
-} |
- |
-void my_close(void *arg) |
-{ |
- int i; |
- TIME tmp; |
- |
- kern_printf("Taskset Execution Time\n\n"); |
- for (i=3; i<MAX_PROC; i++){ |
- if (!jet_getstat(i, NULL, &tmp, NULL, NULL)) |
- kern_printf("Task Name : %s - Max Time : %d\n", proc_table[i].name, (int)tmp); |
- } |
-} |
- |
- |
-TASK write() |
-{ |
- JOY_STATE jsa; |
- |
- while (1) { |
- get_joystick_A(&jsa); |
- place(2,4); |
- cprintf("The X coord. is: %3d and the Y coord. is: %3d of the joystick1\n", jsa.x, jsa.y); |
- place(2,12); |
- cprintf("The button 1, joystick 1 is: %3d\n", jsa.b1); |
- place(2,14); |
- cprintf("The button 2, joystick 1 is: %3d\n", jsa.b2); |
- |
- task_endcycle(); |
- } |
- return 0; |
-} |
- |
-int main(int argc, char **argv) |
-{ |
- SOFT_TASK_MODEL ms; |
- KEY_EVT k; |
- TIME seme; |
- |
- JOY_BOUND jb; |
- |
- k.flag = CNTR_BIT; |
- k.scan = KEY_C; |
- k.ascii = 'c'; |
- keyb_hook(k,endfun); |
- |
- k.flag = CNTL_BIT; |
- k.scan = KEY_C; |
- k.ascii = 'c'; |
- keyb_hook(k,endfun); |
- |
- seme = sys_gettime(NULL); |
- srand(seme); |
- |
- sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT); |
- |
- clear(); |
- |
- if (get_joystick_bound_A(&jb)) { |
- perror("Could not find Joystick."); |
- sys_end(); |
- } |
- |
- place(2,2); |
- cprintf("The X bounds are: [ %3d - %3d ] and the Y bounds are: [ %3d - %3d ]\n", jb.x_min,jb.x_max,jb.y_min,jb.y_max); |
- |
- soft_task_default_model(ms); |
- soft_task_def_level(ms,1); |
- soft_task_def_ctrl_jet(ms); |
- soft_task_def_met(ms,100); |
- soft_task_def_period(ms,10000); |
- soft_task_def_usemath(ms); |
- pid = task_create("Write", write, &ms, NULL); |
- if (pid == NIL) { |
- perror("Could not create task <Write>"); |
- sys_end(); |
- } else { |
- task_activate(pid); |
- } |
- |
- return 0; |
-} |
/rel_0_5/joy/joy_test.c |
Property changes: |
Deleted: svn:executable |
## -1 +0,0 ## |
-* |
\ No newline at end of property |
Index: rel_0_5/joy/joy_grx.c |
=================================================================== |
--- rel_0_5/joy/joy_grx.c (revision 1657) |
+++ rel_0_5/joy/joy_grx.c (nonexistent) |
@@ -1,177 +0,0 @@ |
- |
-/* |
- * Project: S.Ha.R.K. |
- * |
- * Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
- * |
- * Authors : Paolo Gai <pj@hartik.sssup.it> |
- * (see authors.txt for full list of hartik's authors) |
- * |
- * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
- * |
- * http://www.sssup.it |
- * http://retis.sssup.it |
- * http://shark.sssup.it |
- */ |
- |
-/* |
- * Copyright (C) 2000 Paolo Gai |
- * |
- * 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 |
- * |
- */ |
- |
-#include <kernel/kern.h> |
-#include <kernel/func.h> |
-#include <stdlib.h> |
-#include <drivers/keyb.h> |
-#include <drivers/glib.h> |
-#include <semaphore.h> |
-#include "modules/sem.h" |
-#include "modules/hartport.h" |
- |
-#include <math.h> |
- |
-#include "joy.h" |
- |
-#define RGB_BLACK rgb16( 0, 0, 0) |
-#define RGB_GRAY rgb16(127,127,127) |
-#define RGB_WHITE rgb16(255,255,255) |
-#define RGB_RED rgb16(255, 0, 0) |
-#define RGB_GREEN rgb16( 0,255, 0) |
-#define RGB_BLUE rgb16( 0, 0,255) |
-#define RGB_YELLOW rgb16(255,255, 0) |
-#define RGB_MAGENTA rgb16(255, 0,255) |
-#define RGB_CYAN rgb16( 0,255,255) |
-#define RGB_D_RED rgb16(127, 0, 0) |
-#define RGB_D_GREEN rgb16( 0,127, 0) |
-#define RGB_D_BLUE rgb16( 0, 0,127) |
-#define RGB_D_YELLOW rgb16(127,127, 0) |
-#define RGB_D_MAGENTA rgb16(127, 0,127) |
-#define RGB_D_CYAN rgb16( 0,127,127) |
- |
-sem_t mx_mat, mx_grf; /* mutex semaphores */ |
-PID pid; |
-JOY_BOUND jb; |
- |
- |
-TASK write() |
-{ |
- int x = 319, y = 239; |
- float dx, dy; |
- JOY_STATE jsa; |
- |
- clear(); |
- |
- dx = 640 / (jb.x_max - jb.x_min); |
- dy = 480 / (jb.y_max - jb.y_min); |
- |
- while (1) { |
- sem_wait(&mx_grf); |
- grx_circle(10 + x, 10 + y, 9, RGB_BLACK); |
- sem_post(&mx_grf); |
- |
- get_joystick_A(&jsa); |
- |
- x = (jsa.x - jb.x_min) * dx; |
- y = (jsa.y - jb.y_min) * dy; |
- |
- sem_wait(&mx_grf); |
- if (jsa.b1==1) |
- grx_circle(10 + dx * x, 10 + dy * y, 9, RGB_RED); |
- else |
- grx_circle(10 + dx * x, 10 + dy * y, 9, RGB_YELLOW); |
- sem_post(&mx_grf); |
- |
- task_endcycle(); |
- } |
- return 0; |
-} |
- |
-void endfun(KEY_EVT *k) |
-{ |
- cprintf("Ctrl-Brk pressed! Ending...\n"); |
- sys_end(); |
-} |
- |
-void my_close(void *arg) |
-{ |
- int i; |
- TIME tmp; |
- |
- grx_close(); |
- kern_printf("Taskset Execution Time\n\n"); |
- for (i=3; i<MAX_PROC; i++){ |
- if (!jet_getstat(i, NULL, &tmp, NULL, NULL)) |
- kern_printf("Task Name : %s - Max Time : %d\n", proc_table[i].name, (int)tmp); |
- } |
-} |
- |
-void init_graph() { |
- grx_box( 0, 0,639,479,RGB_BLACK); |
-} |
- |
-int main(int argc, char **argv) |
-{ |
- SOFT_TASK_MODEL ms; |
- KEY_EVT k; |
- TIME seme; |
- int modenum; |
- |
- k.flag = CNTR_BIT; |
- k.scan = KEY_C; |
- k.ascii = 'c'; |
- keyb_hook(k,endfun); |
- |
- k.flag = CNTL_BIT; |
- k.scan = KEY_C; |
- k.ascii = 'c'; |
- keyb_hook(k,endfun); |
- |
- sem_init(&mx_mat,0,1); |
- sem_init(&mx_grf,0,1); |
- |
- seme = sys_gettime(NULL); |
- srand(seme); |
- |
- sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT); |
- |
- if (get_joystick_bound_A(&jb)) { |
- perror("Could not find Joystick."); |
- sys_end(); |
- } |
- |
- grx_init(); |
- modenum = grx_getmode(640, 480, 16); |
- grx_setmode(modenum); |
- init_graph(); |
- |
- soft_task_default_model(ms); |
- soft_task_def_level(ms,1); |
- soft_task_def_ctrl_jet(ms); |
- soft_task_def_met(ms,100); |
- soft_task_def_period(ms,10000); |
- soft_task_def_usemath(ms); |
- pid = task_create("Write", write, &ms, NULL); |
- if (pid == NIL) { |
- grx_close(); |
- perror("Could not create task <Write>"); |
- sys_end(); |
- } else { |
- task_activate(pid); |
- } |
- |
- return 0; |
-} |
/rel_0_5/joy/joy_grx.c |
Property changes: |
Deleted: svn:executable |
## -1 +0,0 ## |
-* |
\ No newline at end of property |
Index: rel_0_5/joy/initfile.c |
=================================================================== |
--- rel_0_5/joy/initfile.c (revision 1657) |
+++ rel_0_5/joy/initfile.c (nonexistent) |
@@ -1,84 +0,0 @@ |
-/* |
- * Project: HARTIK (HA-rd R-eal TI-me K-ernel) |
- * |
- * Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
- * Gerardo Lamastra <gerardo@sssup.it> |
- * |
- * Authors : Paolo Gai <pj@hartik.sssup.it> |
- * (see authors.txt for full list of hartik's authors) |
- * |
- * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
- * |
- * http://www.sssup.it |
- * http://retis.sssup.it |
- * http://hartik.sssup.it |
- */ |
- |
-/* |
- * Copyright (C) 2000 Paolo Gai |
- * |
- * 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 |
- * |
- */ |
- |
-#include <kernel/kern.h> |
-#include <kernel/func.h> |
-#include <stdlib.h> |
-#include <drivers/keyb.h> |
-#include <semaphore.h> |
-#include "modules/sem.h" |
-#include "modules/hartport.h" |
- |
-#include "modules/edf.h" |
-#include "modules/cbs.h" |
-#include "modules/rr.h" |
-#include "modules/dummy.h" |
- |
-/*+ sysyem tick in us +*/ |
-#define TICK 0 //300 |
- |
-/*+ RR tick in us +*/ |
-#define RRTICK 300 |
- |
-TIME __kernel_register_levels__(void *arg) |
-{ |
- struct multiboot_info *mb = (struct multiboot_info *)arg; |
- |
- EDF_register_level(EDF_ENABLE_ALL); |
- CBS_register_level(CBS_ENABLE_ALL, 0); |
- RR_register_level(RRTICK, RR_MAIN_YES, mb); |
- dummy_register_level(); |
- |
- SEM_register_module(); |
- |
- return TICK; |
-} |
- |
-TASK __init__(void *arg) |
-{ |
- struct multiboot_info *mb = (struct multiboot_info *)arg; |
- |
- KEYB_PARMS kparms = BASE_KEYB; |
- |
- HARTPORT_init(); |
- |
- //keyb_set_map(itaMap); |
- keyb_def_ctrlC(kparms, NULL); |
- KEYB_init(&kparms); |
- |
- __call_main__(mb); |
- |
- return (void *)0; |
-} |
/rel_0_5/joy/initfile.c |
Property changes: |
Deleted: svn:executable |
## -1 +0,0 ## |
-* |
\ No newline at end of property |
Index: rel_0_5/robots/initfile.c |
=================================================================== |
--- rel_0_5/robots/initfile.c (revision 1657) |
+++ rel_0_5/robots/initfile.c (nonexistent) |
@@ -1,104 +0,0 @@ |
-/* |
- * Project: S.Ha.R.K. |
- * |
- * Coordinators: |
- * Giorgio Buttazzo <giorgio@sssup.it> |
- * Paolo Gai <pj@gandalf.sssup.it> |
- * |
- * Authors : |
- * Paolo Gai <pj@gandalf.sssup.it> |
- * (see the web pages for full authors list) |
- * |
- * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
- * |
- * http://www.sssup.it |
- * http://retis.sssup.it |
- * http://shark.sssup.it |
- */ |
- |
-/** |
- ------------ |
- CVS : $Id: initfile.c,v 1.1.1.1 2004-05-24 18:03:41 giacomo Exp $ |
- |
- File: $File$ |
- Revision: $Revision: 1.1.1.1 $ |
- Last update: $Date: 2004-05-24 18:03:41 $ |
- ------------ |
-**/ |
- |
-/* |
- * Copyright (C) 2001 Paolo Gai |
- * |
- * 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 |
- * |
- */ |
- |
-#include "kernel/kern.h" |
-#include "modules/edf.h" |
-#include "modules/cbs.h" |
-#include "modules/rr.h" |
-#include "modules/dummy.h" |
- |
-#include "modules/sem.h" |
-#include "modules/hartport.h" |
-#include "modules/cabs.h" |
-#include "drivers/keyb.h" |
- |
-/*+ sysyem tick in us +*/ |
-#define TICK 0 |
- |
-/*+ RR tick in us +*/ |
-#define RRTICK 2000 |
- |
-void read_cfgfile(int); |
- |
-int argc; |
-char *argv[100]; |
- |
-void read_cfg_file(int argc, char **argv); |
- |
-int main(int argc, char **argv); |
- |
-TIME __kernel_register_levels__(void *arg) |
-{ |
- struct multiboot_info *mb = (struct multiboot_info *)arg; |
- |
- EDF_register_level(0); |
- CBS_register_level(0, 0); |
- RR_register_level(RRTICK, RR_MAIN_YES, mb); |
- dummy_register_level(); |
- |
- SEM_register_module(); |
- |
- CABS_register_module(); |
- |
- __compute_args__(mb, &argc, argv); |
- |
- read_cfg_file(argc, argv); |
- |
- return TICK; |
-} |
- |
-TASK __init__(void *arg) |
-{ |
- KEYB_PARMS kparms = BASE_KEYB; |
- |
- HARTPORT_init(); |
- |
- keyb_def_ctrlC(kparms, NULL); |
- KEYB_init(&kparms); |
- |
- return (void *)main(argc,argv); |
-} |
Index: rel_0_5/robots/lab1m.txt |
=================================================================== |
--- rel_0_5/robots/lab1m.txt (revision 1657) |
+++ rel_0_5/robots/lab1m.txt (nonexistent) |
@@ -1,9 +0,0 @@ |
-1 |
-540 380 15 270 |
-380 300 400 5 |
-430 060 460 400 0 |
-350 100 430 130 0 |
-000 100 300 130 0 |
-150 200 430 230 0 |
-120 300 150 400 0 |
- |
Index: rel_0_5/robots/mouse2.c |
=================================================================== |
--- rel_0_5/robots/mouse2.c (revision 1657) |
+++ rel_0_5/robots/mouse2.c (nonexistent) |
@@ -1,933 +0,0 @@ |
-/*--------------------------------------------------------------*/ |
-/* 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); |
- } |
-} |
Index: rel_0_5/robots/lab.txt |
=================================================================== |
--- rel_0_5/robots/lab.txt (revision 1657) |
+++ rel_0_5/robots/lab.txt (nonexistent) |
@@ -1,10 +0,0 @@ |
-2 |
-530 380 15 270 |
-480 380 15 270 |
-380 300 400 5 |
-430 060 460 400 0 |
-350 100 430 130 0 |
-000 100 300 130 0 |
-150 200 430 230 0 |
-120 300 150 400 0 |
- |
Index: rel_0_5/robots/readme |
=================================================================== |
--- rel_0_5/robots/readme (revision 1657) |
+++ rel_0_5/robots/readme (nonexistent) |
@@ -1,9 +0,0 @@ |
-Robots demo |
------------ |
- |
-This demo has been developed in the University of Alveiro, Portugal. |
- |
-See the included PDF files for more informations. |
- |
-Paolo |
-pj@sssup.it |
Index: rel_0_5/robots/makefile |
=================================================================== |
--- rel_0_5/robots/makefile (revision 1657) |
+++ rel_0_5/robots/makefile (nonexistent) |
@@ -1,16 +0,0 @@ |
-# |
-# |
-# |
- |
-ifndef BASE |
-BASE=../.. |
-endif |
-include $(BASE)/config/config.mk |
- |
-PROGS = mouse2 |
- |
-include $(BASE)/config/example.mk |
- |
-mouse2: |
- make -f $(SUBMAKE) APP=mouse2 INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __GRX__" |
- |
Index: rel_0_5/robots/lab3.txt |
=================================================================== |
--- rel_0_5/robots/lab3.txt (revision 1657) |
+++ rel_0_5/robots/lab3.txt (nonexistent) |
@@ -1,11 +0,0 @@ |
-3 |
-540 380 15 270 |
-510 380 15 270 |
-480 380 15 270 |
-380 300 400 5 |
-430 060 460 400 0 |
-350 100 430 130 0 |
-000 100 300 130 0 |
-150 200 430 230 0 |
-120 300 150 400 0 |
- |
Index: rel_0_5/auto/tracks/brazil.raw |
=================================================================== |
--- rel_0_5/auto/tracks/brazil.raw (revision 1657) |
+++ rel_0_5/auto/tracks/brazil.raw (nonexistent) |
@@ -1,2 +0,0 @@ |
-
- ! " |
---|