WebSVN - shark - Rev 1664 - /unsupported/trunk/imit2004/car/carfinalcab2.c

#include "carfinal.h"
#include<modules/cabs.h>

/*--adding petrol station to cabs lets make it work baby!!----*/
/*----------------------------------------------------------------------
graphics functions:
1. hide_car: it repaints the screen after the car moves from an old position
2. draw_car: draws the car at its current position.The color of cars vary with their speeds

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

typedef struct{
int x;
int y;
}Position;
Position ps;
CAB fuel;

/*[(MAX_C+2)]; all cars are readers and keyboard and mouse are writers*/

int cari = 0;
sem_t carim;

Position itemp;
char *mcabptr,*kcabptr;

int hide_car(int x,int y)
{
mutex_lock(&display_mutex);
grx_box(x,y,x+10,y+10,background);
mutex_unlock(&display_mutex);
return 0;

}

int draw_car(int x,int y,int vmax)
{
int x1 = (x+10)/2 - 2;
int y1 = (y+10)/2;
int x2 = (x+10)/2 + 2;
int col;
switch(vmax)
{
case 4:
{
col = white;
break;
}
case 6:
{
col = gray;
break;
}
case 8:
{
col = blue;
break;
}
default:
col = yellow;
}
mutex_lock(&display_mutex);
grx_box(x,y,x1,y1,background);
grx_box(x,y,x+10,y+10,col);
grx_box(x,y,x2,y1,background);
mutex_unlock(&display_mutex);
return 0;
}
/*--------------------Graphics functions End--------------------------------*/

/*-------------------------------------------------------------------------
TASK car:
Description:
Modeling the car as a hard task
Time period:car_period as in carfinal.h
wcet:car_wcet as in carfinal.h

possible improvements:
The task could be subdivided into four tasks:
1. update(samples the current position of the physical task
2. Target find: gives the direction of destination(petrol station in this case)
3. Collision : tries to inform the car about impending collision
4. Display: tries to draw the car on the screen based on the desicions taken by update
In this model update and target are hard tasks while Display and collision are soft tasks
-------------------------------------------------------------------------*/

TASK Car(void *arg)
{

/*organise the declarations*/

int carid;
int angv;
char tmyprint[40]; /*characteristic of car*/
int vc = 0,vmax,acc;
float slope;
int collisioncount = 0;
/*for writing on the ports ..collision handling*/
char namex[7],cordx[4]; //namex is :carx+<1..>cordx: the sampled coordiate data on screen*/
char namey[7],cordy[4];
char tmyprintx[40], tmyprinty[40];

/*****************/

/*position and color for the graphic*/
int x, y;
int ox, oy;
float dx, dy, da;
double alpha;
Position carps;
char *carcabptr;

char carname[40];

enum mybool warning,collide;
unsigned long colw,colg,colr,coly;

//cprintf("color value: %ul",i);

collide = warning = false;

/***this section:
1. increment carcount
2. give unique id to car
3. creates ports for writing(sampling) its own position
*****/

carid = carcount++;
sprintf (namex,"carx%d",carid); //converting the carid to string
sprintf(namey,"cary%d",carid);
px[carid] = port_create(namex,7*sizeof(char),1,STICK,WRITE);
port_send(px[carid],"0",BLOCK);
py[carid] = port_create(namey,7*sizeof(char),1,STICK,WRITE);
port_send(py[carid],"0",carid);

/*******************/

vmax = VEL- 2*(carid%3);
cprintf(" vmax = %d" ,vmax);
cprintf(" carid = %d",carid); //possible values: 4,6,8
angv = 30 + 15*(carid%3); //possible values: 30,45,60 depending on carid
cprintf(" angv = %d",angv);
acc = 1+ carid%2; //possible values of acceleration : 1,2
/*********/
cprintf("here i am 1");
x= randomnumber(XMIN,XMAX); /*if out of range start in the middle*/ y=randomnumber(YMIN,YMAX);
da =randomangle();
cprintf("here i am 2");

//converting to radians
alpha = da * PI/180;
colw = rgb16(255,255,255); /* color car*/
colg = rgb16(0,255,0);
colr = rgb16(255,0,0);
coly= rgb16(0,255,255);
/*if(carid == 1)
mutex_lock(&display_mutex);
grx_text("I am here!!",XMIN,YMENU +80,30,0 );
mutex_unlock(&display_mutex);
cprintf("before the while loop");*/
task_endcycle();
while (1/*collide == false*/)
{
ox = x; /*saving the previous values*/
oy = y;

carcabptr = cab_getmes(fuel);
memcpy(&carps,carcabptr,sizeof(Position));
cab_unget(fuel,carcabptr);

Px = carps.x;
Py = carps.y;

if(collisioncount == 0)
{

if(vc != vmax)
{
vc+=acc;
if(vc>vmax)
vc = vmax;
}
if(Px - x != 0)
{
slope = (Py - y)/(Px -x);
alpha = atan(slope);
if((Px-x)<0)
{
alpha = alpha+PI; //chaged here..in this verison
}

}
else
alpha = PI/2;

}
else
{
collisioncount--;
if(collisioncount == 0)
warning = false;
}

/*Collision Handling :syncronising with the tasks which have written to the ports*/
/**count is guaranteed to be equal to that of those taks that have written to the port*/
func_collision(carid, x, y,&collisioncount,&collide,&warning,&alpha);
/***collsiion handling ends**********/

if(warning == true)
{
if(vc >vmax/2)
{
vc = vc - acc;
if(vc<vmax/2)
vc = vmax/2;
}
}
dx = (float)(vc * cos(alpha)); //round of instead of error
dy = (float)(vc * sin(alpha));
x += dx;
y += dy;

/*now check one thing :
1. is the new location close to petrol station
*/
/*checking if it reaches boundary*/
boundary_check(&x,&y);

/*checking if it reaches petrol station*/
if((x == Px||x== Px-1||x== Px+1) && (y == Py||y == Py-1 ||y == Py+1))
{
x = randomnumber(XMIN,XMAX);
y = randomnumber(YMIN,YMAX);
da = randomangle();
alpha = da * PI/180;
sprintf(carname,"%d",carid);
strcat(carname,"reached the station");
mutex_lock(&display_mutex);
grx_text(carname,XMIN,YMENU +70,30,0 );
mutex_unlock(&display_mutex);
/**behaviour expected: starts again at a diff position */
}

/*checking if it has collided*/
if(collide == true)
{
/*sem_wait(&carim);
cari--;
sem_post(&carim);*/

x = randomnumber(XMIN,XMAX);
y=randomnumber(YMIN,YMAX);
da =randomangle();
alpha = da * PI/180;
sprintf(carname,"%d",carid);
strcat(carname,"collided!!");
mutex_lock(&display_mutex);
grx_text(carname,XMIN, YMENU +50,30,0 );
mutex_unlock(&display_mutex);
collide = false;
}

sprintf(cordx, "%d", x); //sampling
sprintf(cordy,"%d",y); //sampling
sprintf(tmyprintx,"X cord=%s ",cordx);
sprintf(tmyprinty,"Y cord=%s ",cordy);
sprintf(tmyprint,"CARID= %d",carid);
strcat(tmyprintx,tmyprinty);
strcat(tmyprint,tmyprintx);
mutex_lock(&display_mutex);
grx_text(tmyprint,XMIN,YMENU +110,30,0 );
mutex_unlock(&display_mutex);
port_send(px[carid],cordx,BLOCK);
port_send(py[carid],cordy,BLOCK);
hide_car(ox, oy);
draw_car(x,y,vmax);
task_endcycle();
}

}
/*----------------------Task Car Ends --------------------------------------*/

/*------------------------------SYSTEM TERMINATION--------------------------
This function is called when the system exits
---------------------------------------------------------------------------*/

void byebye(void *arg)
{
grx_close();
cprintf("Termination.");
}

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

/*---------------------------------------------------------------------
TASK Repaint_border
Description: created to repaint the borders

NAME repaint_border
met: car_wcet (could be changed if required)
Type:Soft Task

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

TASK repaint_border(void *arg)
{
Position r;
char *rcabptr;

while(1)
{
rcabptr = cab_getmes(fuel);
memcpy(&r,rcabptr,sizeof(Position));
cab_unget(fuel,rcabptr);

mutex_lock(&display_mutex);
grx_rect(XMIN-D, YMIN-D, XMAX+D+2, YMAX+D+2, rgb16(0,255,0));
grx_box(r.x,r.y,r.x+10,r.y+10,rgb16(255,255,0));
mutex_unlock(&display_mutex);
task_endcycle();
}
}
/*----------------------------------TASK Repaint ends ----------------*/

/*-------io device handler functions---------------------------*/
/***handler for keyboard***/
void myhook(KEY_EVT *key1)
{
int x,y,ox,oy;
ox=itemp.x;
oy=itemp.y;

mutex_lock(&display_mutex);
grx_rect(ox, oy, ox+10, oy+10, rgb16(0,0,0));
mutex_unlock(&display_mutex);

switch(key1->ascii)
{
case 'k':{itemp.y++;;break;}
case 'j':{itemp.x--;break;}
case 'i':{itemp.y--;break;}
case 'l':{itemp.x++;break;}
}
x = itemp.x;
y = itemp.y;

kcabptr = cab_reserve(fuel);
memcpy(kcabptr,&itemp,sizeof(Position));
cab_putmes(fuel,kcabptr);

mutex_lock(&display_mutex);
grx_box(x, y, x+10, y+10, rgb16(255,255,0));
mutex_unlock(&display_mutex);
}

/**********mouse control*********************/
void mouseHandler(MOUSE_EVT* evt)
{
int x,y,ox,oy;
char mx[20],my[20];
ox=itemp.x;
oy=itemp.y;

x = itemp.x = evt->x;
y = itemp.y = evt->y;

mutex_lock(&display_mutex);
grx_box(ox,oy,ox+10,oy+10,0);
mutex_unlock(&display_mutex);

mcabptr = cab_reserve(fuel);
memcpy(mcabptr,&itemp,sizeof(Position));
cab_putmes(fuel,mcabptr);

mutex_lock(&display_mutex);
grx_box(x,y,x+10,y+10,rgb16(255,255,0));
mutex_unlock(&display_mutex);

sprintf(mx,"x = %d", x);
sprintf(my,"y = %d",y);
}
/*-------------------io device handler functions end---------------------*/

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

MAIN function :

this function is called when the application starts.
it does the following:
1.setup the keyborad hook
2.setup the mouse hook
3.create tasks:There are five categories of tasks in teh system.three are for jetcontrol.One for Repainting.One for the car.The first four tasks are soft taks while the car task is hard task.

-------------------------------------------------------------------------------*/
int main(int argc, char **argv)
{
HARD_TASK_MODEL m; /*for task Car*/
SOFT_TASK_MODEL r; /*for task Repaint*/
carcount = 0; /*current number of cars in the system*/
char c; /* character from keyboard */
/* number of tasks created */
TIME sem; /* used to init the random seed */
KEY_EVT key;
MOUSE_PARMS params = BASE_MOUSE;
int forMouse;

char cabname[] = "fuelcab"; /*name of the cab..only for debugging*/
int cno = MAX_C + 3;
char *cabptr;
Position temp;
temp.x = 200; /*initial location of the fuel station*/
temp.y = 250;
itemp = temp;
sem_init(carim,0,1);
if(app_mutex_init(&display_mutex)<0)
cprintf("mutex attribute can't be initialised");
else
cprintf("mutex attribute properly initialised");

/*creating and initialising the cab*/
fuel = cab_create(cabname,sizeof(Position),cno);

cabptr = cab_reserve(fuel);
memcpy(cabptr,&temp,sizeof(Position));
cab_putmes(fuel,cabptr);
/*-------------------cab handling ends-----------------*/
/*key scan for changing the position of the petrol station*/ key.ascii = 'k';
key.scan = KEY_K;
key.flag=CNTL_BIT;
keyb_hook(key,myhook);
key.ascii = 'j';
key.scan = KEY_J;
key.flag=CNTL_BIT;
keyb_hook(key,myhook);
key.ascii = 'i';
key.scan = KEY_I;
key.flag=CNTL_BIT;
keyb_hook(key,myhook);
key.ascii = 'l';
key.scan = KEY_L;
key.flag=CNTL_BIT;
keyb_hook(key,myhook);
/*****************end keyboard*******************/

/*********mouse handling *************/
forMouse = mouse_init(&params);
mouse_limit(XMIN-D, YMIN-D,XMAX+D+2,YMAX+D+2);
mouse_on();
if(forMouse != 0)
{
grx_text("Mouse cannot be initialised",XMIN, YMENU+110, 13, 0);
}
mouse_on();
mouse_grxcursor(ENABLE);
mouse_position(200, 200);
mouse_hook(mouseHandler);

/********************mouse handling ends*******************/

/* Set the closing function */
sys_atrunlevel(byebye, NULL, RUNLEVEL_BEFORE_EXIT);

/* graphic card Initialization */
if (grx_init() < 1)
{
sys_abort(1);
}

if (grx_open(640, 480, 16) < 0)
{
cprintf("GRX Err");
sys_abort(1);
}
//grx_close /*uncomment it when you want to debug in text mode*/

mutex_lock(&display_mutex);
grx_rect(XMIN-D, YMIN-D, XMAX+D+2, YMAX+D+2, rgb16(0,255,0));
grx_text("Car Game", XMIN+50, YMENU+10, rgb16(255,255,0),rgb16(0,0,0));
grx_text("Press SPACE bar create a new car", XMIN,YMENU+20, rgb16(255,0,0),rgb16(0,0,0));
grx_text("ESC exit Game", XMIN, YMENU+40, rgb16(255,0,0),rgb16(0,0,0));
grx_text("Use Mouse to Move the Petrol Station", XMIN, YMENU+30, rgb16(255,0,0),rgb16(0,0,0));
mutex_unlock(&display_mutex);

scenario_jetcontrol();
init_jetcontrol();

/*Px = randomnumber(XMIN/2,XMAX/2); */ //randomise it later;
/*Py = randomnumber(YMIN/2,YMAX/2); */ //rqaandomise it later.
grx_box(temp.x, temp.y, temp.x+10, temp.y+10,rgb16(255,255,0));
/* The program waits a space to create a car */
c = keyb_getch(BLOCK);
/* randomize!!!! */
sem = sys_gettime(NULL);
srand(sem);

soft_task_default_model(r);
soft_task_def_arg (r, (void *)cari);
soft_task_def_level(r,1);
soft_task_def_period(r, car_period);
soft_task_def_met(r, car_wcet);
soft_task_def_ctrl_jet(r);

rid = task_create("repaint", repaint_border, &r, NULL);

if(task_activate(rid)==-1)
grx_text("Unable to repaint", XMIN, YMENU+130, 12, 0);

do
{
if ((c == ' ') && (cari < MAX_C))
{
hard_task_default_model(m);
hard_task_def_ctrl_jet (m);
hard_task_def_arg (m, (void *)cari);
hard_task_def_wcet (m, car_wcet);
hard_task_def_mit (m, car_period);
hard_task_def_group (m, CARGROUP);
hard_task_def_usemath (m);

pid = task_create("car", Car, &m, NULL);

if (pid == NIL)
{
grx_close();
perror("Could not create task <car>");
sys_abort(1);
}

if(task_activate(pid)==-1)
{
grx_close();
perror("Error activating the task!!");
sys_abort(1);
}

/*sem_wait(&carim);*/
cari++;
/*sem_post(&carim);*/
}
c = keyb_getch(BLOCK);

}while (c != ESC);

sys_end();
return 0;
}

/*------------------------------main ends-------------------------------------*/

/*------------------------------------------------------------------------
helper functions:
1.randomnumber
2.randomangle
3. boundary check
--------------------------------------------------------------------------*/
int randomnumber(int min, int max)
{ int temp;
temp=rand()%((min+max)/2); //old and new values to some random initial value
if(temp > max || temp < min)
temp = min+max/2;
return temp;
}

int randomangle()
{ int temp;
temp=rand()%(2*ANG) - ANG;
if (temp > 360) temp -= 360;
if (temp < 0) temp+= 360;
return temp;
}

int boundary_check(int *x, int *y)
{

int outx, outy;
outx = (*x >= XMAX) || (*x <= XMIN);
outy = (*y >= YMAX) || (*y <= YMIN);
if (outx || outy)
{
if(*x >= XMAX) *x = XMIN+1;
if(*y >=YMAX) *y = YMIN+1;
if(*x<=XMIN) *x=XMAX-1;
if(*y<=YMIN) *y=YMAX-1;
}
return 0;
}
/*------------------helper funtions end------------------------------------*/

/*-------------------------------------------------------------------------------
Function name:func_collision
description:
the collsion avoidance strategy is not very robust.This is true even in real life if we take unpredictable situations.the car should however react as soon as possible.so we have to check it there is a collision and terminate the task then

input parameters:
carid:
x,y : position of the car
collisioncount: current value of collision count
output parameters:
collisioncount: current value of coolision count
collide: set if the car has to collide
warning: set if the car is on collision course
alpha: the current value of angle in radians
return type:
void

the function for handling the collsion avoidance
this function uses a lot of parameters at present as we tried to reorganise our code ..it can definately be improoved!!
------------------------------------------------------------------------------*/
void func_collision(int carid, int x, int y, int *collisioncount,enum mybool *collide,enum mybool *warning, double *alpha)
{
int ti,xoc,yoc;
PORT ptx,pty;
char tmyprint[40];
int count;
count=carcount;
char tnamex[7],tnamey[7],tcordx[4],tcordy[4];
float angdiff, diff;
float angv;
angv = 30 + 15*(carid%3);
double collisionangle;

count=carcount;
for(ti=0; ti<count; ti++)
{
if(ti==carid)
continue; //car will always be close to itself!!
sprintf (tnamex,"carx%d",ti);
sprintf(tnamey,"cary%d",ti);
ptx = port_connect(tnamex, 7*sizeof(char), STICK, READ);
pty = port_connect(tnamey, 7*sizeof(char),STICK, READ);
port_receive(ptx, tcordx, BLOCK);
port_receive(pty, tcordy, BLOCK);
sscanf(tcordx,"%d",&xoc);
sscanf(tcordy,"%d",&yoc);
port_disconnect(ptx);
port_disconnect(pty);
diff = sqrt((x-xoc)*(x-xoc) + (y-yoc)*(y-yoc)); //distance between cars //computing angle of approach of cars in range(collision angle)
if(x-xoc == 0)
{
if(y-yoc > 0)
collisionangle = PI/2;
else
collisionangle = 3*PI/2;
}
else
{
collisionangle = atan((y-yoc)/(x-xoc)); //angle of aproaching
angdiff = abs(*alpha - collisionangle)* 180/PI; //convering to degrees
/*checking if there is a chancee of collsion or collsion*/
if(diff < MINDIST && angdiff < 10)
{
if(diff <= COLDIST && angdiff < 5)
{
*collide = true;
break;
}
*collisioncount = 4;
if(*warning != true)
{
*warning = true;
if(*alpha >= collisionangle)
*alpha = *alpha+angv;
if(*alpha<collisionangle)
*alpha = *alpha -angv;
}
sprintf(tmyprint,"%d",carid);
/*mutex_lock(&display_mutex);
grx_text(tmyprint,XMIN+20,YMENU +60,25,0);
mutex_unlock(&display_mutex);*/
break; //get out of the for loop for the first collsion detected
}

}
}

}

/*----------------------------func_collision ends----------------------------*/

Subversion Repositories shark

(root)/unsupported/trunk/imit2004/car/carfinalcab2.c @ 1682 - Rev 1664