Пример #1
0
void sensor_fusion(void *arg)
{
	RTIME now;
	int i,sum, print,k, count;
        /*
         * Arguments: &task (NULL=self), start time, period 
         */
        rt_task_set_periodic(NULL, TM_NOW, 16666666);
	count =0;
	while (isRunning) {
		count++;
                rt_task_wait_period(NULL);
		rt_mutex_acquire(&mutex_acc,TM_INFINITE);
		rt_mutex_acquire(&mutex_gyro,TM_INFINITE);
		k=gyroIndex;
		sum=0; print = 0;
   		for (i=0; i<5; i++){
	 		sum = sum + gyro_sample[k];
			k--;
		}

		print = ((sum/5) + acc_sample)/2;

		rt_mutex_release(&mutex_gyro);
		rt_mutex_release(&mutex_acc);
		
		printlog[logIndex] = print;
		logIndex++;

		if(count == 50)
			isRunning = 0;
	}
}
	int acquireWrapper(bool blocking)
	{
		const RTIME timeout = blocking ? TM_INFINITE : TM_NONBLOCK;
		int ret;

		ret = rt_mutex_acquire(&m, timeout);
		if (ret == -EPERM) {
			// become real-time, then try again

			// Allocate a new RT_TASK struct, and then forget the pointer. This
			// leak allows us to avoid ownership issues for the RT_TASK, which
			// shouldn't necessarily be deleted when the mutex is released, or when
			// the mutex is deleted, etc.. It is a small overhead that happens (at
			// most) once per thread. If needed, we can always get the pointer back
			// by calling rt_task_self().
			rt_task_shadow(new RT_TASK, NULL, 10, 0);

			ret = rt_mutex_acquire(&m, timeout);
		}
		if (ret != 0) {
			return ret;
		}

		if (lockCount == 0) {
			int oldMode;
			ret = rt_task_set_mode(0, T_WARNSW, &oldMode);
			if (ret != 0) {
				throw std::runtime_error("thread::detail::mutex_impl::acquireWrapper(): Could not set T_WARNSW mode.");
			}
			leaveWarnSwitchOn = oldMode & T_WARNSW;
		}
		++lockCount;

		return ret;
	}
Пример #3
0
void addPong(){
    if (nPongs < MAX_PONGS){
        pongs[nPongs].leftCorner.point_x = ((resolution_w-resolution_x)/2 - (pongLength/2));
        pongs[nPongs].leftCorner.point_y = resolution_y;
        oppoentPointLocationX = ((resolution_w-resolution_x)/2 - (pongLength/2));

        rt_mutex_acquire(&txMutex[E_ADD], TM_INFINITE);
	memset(pongData[0], 0, sizeof(pongData[0]));
        sprintf(pongData[0], "pong;%d;",pongs[nPongs].leftCorner.point_x);
        sprintf(pongData[0] + strlen(pongData[0]), 
		"%d;%d;%d;", pongs[nPongs].leftCorner.point_y,pongLength,pongs[nPongs].height);
	padString(pongData[0]);
        rt_mutex_release(&txMutex[E_ADD]);
        
        pongs[nPongs].length = pongLength;
        nPongs++;
        pongs[nPongs].leftCorner.point_x =((resolution_w-resolution_x)/2 - (pongLength/2));
        pongs[nPongs].leftCorner.point_y = DEFAULT_HEIGHT;
        locationPointOfX = ((resolution_w-resolution_x)/2 - (pongLength/2)); 

	rt_mutex_acquire(&txMutex[E_ADD], TM_INFINITE);
	memset(pongData[1], 0, sizeof(pongData[1]));
        sprintf(pongData[1], "pong;%d;",pongs[nPongs].leftCorner.point_x);
        sprintf(pongData[1] + strlen(pongData[1]), 
		"%d;%d;%d;", pongs[nPongs].leftCorner.point_y,pongLength,pongs[nPongs].height);
	padString(pongData[1]);
	rt_mutex_release(&txMutex[E_ADD]);
        nPongs++;
    }
}
Пример #4
0
void computationTask(long arg)
{
	usleep(1);
	while(1){
		int i = 0;
		rt_mutex_acquire(&mutex_donnee,TM_INFINITE);	

		while(tabDonnePris==1){
			rt_mutex_release(&mutex_donnee);
			rt_sem_p(&sem_donnee,TM_INFINITE);
			rt_mutex_acquire(&mutex_donnee,TM_INFINITE);
		}
		tabDonnePris = 1;
		
		rt_mutex_acquire(&mutex_obs,TM_INFINITE);	
		while(tabObstaclePris==1){
		  rt_mutex_release(&mutex_obs);
		  rt_sem_p(&sem_obs,TM_INFINITE);
		  rt_mutex_acquire(&mutex_obs,TM_INFINITE);
		}
		tabObstaclePris = 1;
		printf("***************** COMPUTATION TASK ******************\n");
		
		for(i = 0; i<SENSOR_SIZE; i++){
			//If the obstacle is farther than 7meters
			if(sensorArray[i] > 7){
				
				obstacle* newObstacle = malloc(sizeof(obstacle));
				newObstacle->distance = sensorArray[i];
				
				//Add the new obstacle at the end of the list
				newObstacle->nxt = NULL;
				
				if(obstacleList == NULL) obstacleList = newObstacle;
				else{
					obstacle* temp = obstacleList;
					if(temp->distance==0)temp->distance=newObstacle->distance;
					else{
						while(temp->nxt != NULL)temp = temp->nxt;
						temp->nxt = newObstacle;
					}
				}
			}			
		}
		tabDonnePris = 0;
		tabObstaclePris = 0;
		
		rt_mutex_release(&mutex_obs);
		rt_mutex_release(&mutex_donnee);
		rt_sem_v(&sem_obs);		
		

		usleep(200000);
		printf("fin compute\n");
	}
}
Пример #5
0
void batterie(void *arg) {
    rt_printf("tconnect : Attente du sémarphore semConnectedRobotbatterie\n");
    rt_sem_p(&semConnectedRobot, TM_INFINITE);
    
    int status=1;
    int niveau_batterie=0;
    DBattery *bat= d_new_battery();
    DMessage *message;
    

    rt_printf("tbatterie : Debut de l'éxecution de periodique à 250ms\n");
    rt_task_set_periodic(NULL, TM_NOW, 250000000);
    
     while (1) {
        rt_task_wait_period(NULL);
        rt_printf("tbatterie : Activation périodique\n");
         
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommMoniteur;
        rt_mutex_release(&mutexEtat);
        
         if (status == STATUS_OK) {
             rt_mutex_acquire(&mutexRobot, TM_INFINITE);
             status=d_robot_get_vbat(robot, &niveau_batterie);
             rt_mutex_release(&mutexRobot);
             
             rt_mutex_acquire(&mutexEtat, TM_INFINITE);
             etatCommRobot=status;
             rt_mutex_release(&mutexEtat);
             
             rt_printf("Niveau de la batterie : %d\n", niveau_batterie);
             rt_printf("Status : %d\n", status);
             
             if(status == STATUS_OK) {
                 message=d_new_message();
                 d_battery_set_level(bat,niveau_batterie);
                 d_message_put_battery_level(message, bat);
                 rt_mutex_acquire(&mutexCom, TM_INFINITE);
                 if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
                        message->free(message);
                }
                 rt_mutex_release(&mutexCom);
                 
             }
             
 
         }
     }
    
}
Пример #6
0
void deplacer(void *arg) {
    int status;

    rt_printf("tmove : Attente du sémaphore semDeplacer\n");
    rt_sem_p(&semDeplacer, TM_INFINITE);
    rt_printf("tmove : Debut de l'éxecution periodique à 200ms\n");
    rt_task_set_periodic(NULL, TM_NOW, 200000000);

    while (1) {
        while (cptCommErr < MAX_ECHECS) {
            /* Attente de l'activation périodique */
            rt_task_wait_period(NULL);
            //rt_printf("tmove : Activation périodique\n");

            rt_mutex_acquire(&mutexEtat, TM_INFINITE);
            status = etatCommRobot;
            rt_mutex_release(&mutexEtat);
            if (status == STATUS_OK) {
                rt_mutex_acquire(&mutexMove, TM_INFINITE);
                switch (move->get_direction(move)) {
                case DIRECTION_FORWARD:
                    status = robot->set_motors(robot, MOTEUR_ARRIERE_LENT, MOTEUR_ARRIERE_LENT);
                    break;
                case DIRECTION_LEFT:
                    status = robot->set_motors(robot, MOTEUR_ARRIERE_LENT, MOTEUR_AVANT_LENT);
                    break;
                case DIRECTION_RIGHT:
                    status = robot->set_motors(robot, MOTEUR_AVANT_LENT, MOTEUR_ARRIERE_LENT);
                    break;
                case DIRECTION_STOP:
                    status = robot->set_motors(robot, MOTEUR_STOP, MOTEUR_STOP);
                    break;
                case DIRECTION_STRAIGHT:
                    status = robot->set_motors(robot, MOTEUR_AVANT_LENT, MOTEUR_AVANT_LENT);
                    break;
                }
                rt_mutex_release(&mutexMove);

                if (status == STATUS_OK) {
                    cptCommErr = 0;
                } else {
                    cptCommErr++;
                    rt_printf("tmove : Erreur de communication avec le robot (%d)\n", cptCommErr);
                }
            }
        }
        comm_err_handler(status);
        rt_sem_p(&semDeplacer, TM_INFINITE);
    }
}
Пример #7
0
void accelerometer(void *arg)
{
	RTIME now;
	int acceptable;
        /*
         * Arguments: &task (NULL=self), start time, period 
         */
        rt_task_set_periodic(NULL, TM_NOW, 33333333);

	while (isRunning) {
		
                rt_task_wait_period(NULL);
		acceptable = 0;
		//acceptable = rand() % (800 - 0 + 1) + 0; // acceleration between 0-50 
		acceptable = rand_lim(20);
		rt_mutex_acquire(&mutex_acc,TM_INFINITE);
		
		if(acceptable > acc_sample)
			acc_sample = acc_sample + 1;

		else if(acceptable < acc_sample)
			acc_sample = acc_sample - 1;
		
		rt_mutex_release(&mutex_acc);
	}
}
Пример #8
0
void connecter(void *arg) {
    DMessage *message = d_new_message();
    int status;

    rt_printf("tconnect : Debut de l'exécution de tconnect\n");

    while (1) {
        rt_printf("tconnect : Attente du sémaphore semConnecterRobot\n");
        rt_sem_p(&semConnecterRobot, TM_INFINITE);
        rt_printf("tconnect : Ouverture de la communication avec le robot\n");
        status = robot->open_device(robot);
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommRobot = status;

        if (status == STATUS_OK) {
            cptCommErr = 0;
            //robot->start_insecurely(robot);
            status = robot->start(robot);
            if (status == STATUS_OK) { /* Demarrage du robot */
                rt_printf("tconnect : Robot démarrer\n");
                rt_sem_v(&semDeplacer);
                rt_sem_v(&semRechargerWatchdog);
                rt_sem_v(&semVerifierBatterie);
            } else { /* Impossible de demarrer le robot, tentative de reinitialisation */
                robot->stop(robot);
                robot->close_com(robot);
            }
        }
        
        rt_mutex_release(&mutexEtat);
        message->put_state(message, status);
        serveur->send(serveur, message);
    }
}
Пример #9
0
int rt_cond_wait_epilogue(RT_MUTEX *mutex, unsigned lockcnt)
{
    int err;
    spl_t s;

    xnlock_get_irqsave(&nklock, s);

    mutex = xeno_h2obj_validate(mutex, XENO_MUTEX_MAGIC, RT_MUTEX);

    if (!mutex) {
        err = xeno_handle_error(mutex, XENO_MUTEX_MAGIC, RT_MUTEX);
        goto unlock_and_exit;
    }

    err = rt_mutex_acquire(mutex, TM_INFINITE);

    if(!err)
        mutex->lockcnt = lockcnt; /* Adjust lockcnt */

unlock_and_exit:

    xnlock_put_irqrestore(&nklock, s);

    return err;
}
Пример #10
0
void move(char str){
     
    int err;
    if(str == 'R'){
          if ((err=illegal_move_check(locationPointOfX, standardPongSpeed)) == 0){
              locationPointOfX = locationPointOfX + standardPongSpeed;
              pongs[player].leftCorner.point_x = locationPointOfX;
              pongs[player].leftCorner.point_y = DEFAULT_HEIGHT;
	  }
          else {
	        locationPointOfX = locationPointOfX - (err * standardPongSpeed);
	        pongs[player].leftCorner.point_x = locationPointOfX;
          }
    }

    else if (str == 'L'){
        if ((err=illegal_move_check(locationPointOfX, standardPongSpeed)) == 0){
              locationPointOfX = locationPointOfX - standardPongSpeed;
              pongs[player].leftCorner.point_x = locationPointOfX;
              pongs[player].leftCorner.point_y = DEFAULT_HEIGHT;
          }
	else {
	      locationPointOfX = locationPointOfX + (-err * standardPongSpeed);
	      pongs[player].leftCorner.point_x = locationPointOfX;
	  }
    }

       rt_mutex_acquire(&txMutex[E_MOVE], TM_INFINITE);
       memset(moveData[0], 0, sizeof(moveData[0]));
       sprintf(moveData[0], "player1;%d;%d;%d;%d;",
               pongs[player].leftCorner.point_x, pongs[player].leftCorner.point_y,pongLength, 
               pongs[player].height);
       padString(moveData[0]);
       rt_mutex_release(&txMutex[E_MOVE]);
}
Пример #11
0
void semWait2(){
	rt_task_sleep(1000000);
	rt_mutex_acquire(&b, 0);
	rt_printf("High pri task: lock b\n");
	busy_wait_ms(1);
	rt_mutex_acquire(&a, 0);
	rt_printf("High pri task: lock a\n");
	busy_wait_ms(2);
	rt_printf("High pri task: release a\n");
	rt_mutex_release(&a);
	rt_printf("High pri task: release b \n");
	rt_mutex_release(&b);
	busy_wait_ms(1);
	
	
}
Пример #12
0
/** Get new frame identifier index and allocate corresponding rx buffer.
 * @param[in] port        = port context struct
 * @return new index.
 */
int ecx_getindex(ecx_portt *port)
{
   int idx;
   int cnt;

   rt_mutex_acquire(&port->getindex_mutex, TM_INFINITE);

   idx = port->lastidx + 1;
   /* index can't be larger than buffer array */
   if (idx >= EC_MAXBUF) 
   {
      idx = 0;
   }
   cnt = 0;
   /* try to find unused index */
   while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF))
   {
      idx++;
      cnt++;
      if (idx >= EC_MAXBUF) 
      {
         idx = 0;
      }
   }
   port->rxbufstat[idx] = EC_BUF_ALLOC;
   if (port->redstate != ECT_RED_NONE)
      port->redport->rxbufstat[idx] = EC_BUF_ALLOC;
   port->lastidx = idx;

   rt_mutex_release(&port->getindex_mutex);
   
   return idx;
}
Пример #13
0
void addBall(){

    if(!ball.created){ //singleton pattern
        ball.centre.point_x = (resolution_w-resolution_x)/2;
        ball.centre.point_y = (resolution_h-resolution_y)/2;
        defaultBalllocationX = (resolution_w-resolution_x)/2;
        defaultBalllocationY =  (resolution_h-resolution_y)/2;

	currentBallOfX = ball.centre.point_x;
        currentBallOfY = ball.centre.point_y;

        // pPanel notify for created pPanel
        balldirectionX = DEFAULT_BALLSPEEDX;
        balldirectionY = DEFAULT_BALLSPEEDY;
        
	rt_mutex_acquire(&txMutex[E_GAME], TM_INFINITE);
	memset(gameData, 0, sizeof(gameData));
	sprintf(gameData, "score;%d;%d;",player_score,opponent_score);
	padString(gameData);
	rt_mutex_release(&txMutex[E_GAME]);
      	
	 memset(ballData, 0, sizeof(ballData));
         sprintf(ballData, "ball;%d;%d;%d;",defaultBalllocationX, defaultBalllocationY, ball.radius);
         padString(ballData);
	 ball.created = true;
    }
}
Пример #14
0
/**
 * Set timer for the next wakeup
 * @param value
 */
void setTimer(TIMEVAL value)
{
	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
	last_timeout_set = value;
	rt_mutex_release(&condition_mutex);
	rt_cond_signal(&timer_set);
}
Пример #15
0
/** Transmit buffer over socket (non blocking).
 * @param[in] port        = port context struct
 * @param[in] idx = index in tx buffer array
 * @return socket send result
 */
int ecx_outframe_red(ecx_portt *port, int idx)
{
   ec_comt *datagramP;
   ec_etherheadert *ehp;
   int rval;
   
   ehp = (ec_etherheadert *)&(port->txbuf[idx]);
   /* rewrite MAC source address 1 to primary */
   ehp->sa1 = htons(priMAC[1]);
   /* transmit over primary socket*/
   rval = ecx_outframe(port, idx, 0);
   if (port->redstate != ECT_RED_NONE)
   {   
      //pthread_mutex_lock( &(port->tx_mutex) );
      rt_mutex_acquire(&port->tx_mutex, TM_INFINITE);
      ehp = (ec_etherheadert *)&(port->txbuf2);
      /* use dummy frame for secondary socket transmit (BRD) */
      datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]);
      /* write index to frame */
      datagramP->index = idx;
      /* rewrite MAC source address 1 to secondary */
      ehp->sa1 = htons(secMAC[1]);
      /* transmit over secondary socket */
      SEND(port->redport->sockhandle, &(port->txbuf2), port->txbuflength2 , 0);
      //pthread_mutex_unlock( &(port->tx_mutex) );
      rt_mutex_release(&port->tx_mutex);
      port->redport->rxbufstat[idx] = EC_BUF_TX;
   }   
   
   return rval;
}
Пример #16
0
void opponentMovement(char strOpponent){
    int err;

    if(strOpponent == 'R'){
          if (!(err=illegal_move_check(oppoentPointLocationX, standardPongSpeed))){
              oppoentPointLocationX = oppoentPointLocationX + standardPongSpeed;
              pongs[opponent].leftCorner.point_x = oppoentPointLocationX;
              pongs[opponent].leftCorner.point_y = resolution_y;
          }
	   else {
	        oppoentPointLocationX = oppoentPointLocationX - (err * standardPongSpeed);
	        pongs[opponent].leftCorner.point_x = oppoentPointLocationX;
          }
    }

    else if (strOpponent == 'L'){
        if (!(err=illegal_move_check(oppoentPointLocationX, standardPongSpeed))){
              oppoentPointLocationX = oppoentPointLocationX - standardPongSpeed;
              pongs[opponent].leftCorner.point_x = oppoentPointLocationX;
              pongs[opponent].leftCorner.point_y = resolution_y;
          }
	else {

	      oppoentPointLocationX = oppoentPointLocationX + (-err * standardPongSpeed);
	      pongs[opponent].leftCorner.point_x = oppoentPointLocationX;
	  }
    }

     rt_mutex_acquire(&txMutex[E_MOVE], TM_INFINITE);
     memset(moveData[1], 0, sizeof(moveData[1]));
     sprintf(moveData[1], "player2;%d;%d;%d;%d;",
         pongs[opponent].leftCorner.point_x, pongs[opponent].leftCorner.point_y,pongLength, pongs[opponent].height);
     padString(moveData[1]);
     rt_mutex_release(&txMutex[E_MOVE]);
}
Пример #17
0
bool Mutex::lock(RTIME timeout){
    int status = rt_mutex_acquire(&mutex, timeout);
    
    switch(status){
        case 0:
            return true;
        case -EINVAL:
            Debug::output("Mutext::lock - mutex is not a mutex descriptor.");
            break;
        case -EIDRM:
            Debug::output("Mutext::lock - mutex is a deleted mutex descriptor, including if the deletion occurred while the caller was sleeping on it.");
            break;
        case -EWOULDBLOCK:
            Debug::output("Mutext::lock - timeout is equal to TM_NONBLOCK and the mutex is not immediately available.");
            break;
        case -EINTR:
            Debug::output("Mutext::lock - rt_task_unblock() has been called for the waiting task before the mutex has become available.");
            break;
        case -ETIMEDOUT:
            Debug::output("Mutext::lock - the mutex cannot be made available to the calling task within the specified amount of time.");
            break;
        case -EPERM:
            Debug::output("Mutext::lock -  this service was called from a context which cannot be given the ownership of the mutex (e.g. interrupt, non-realtime context).");
            break;
        default:
            Debug::output("Mutext::lock - Unknown error");
            break;
    }
    
    return false;
}
Пример #18
0
void gyroscope(void *arg)
{
	RTIME now;
	int randG, i, prev;
	gyroIndex = 0;

        /*
         * Arguments: &task (NULL=self), start time, period 
         */
        rt_task_set_periodic(NULL, TM_NOW, 3333333);

	while (isRunning) {
		for(i=0; i<BUFFERSIZE; i++){
                	rt_task_wait_period(NULL);
			//randG = rand() % (361 - 0 + 1) + 0; // angle between 0-361
			randG = rand_lim(360);
			
			rt_mutex_acquire(&mutex_gyro,TM_INFINITE);
			//printf("Random: %d\n", randG);
			prev = gyroIndex;
			gyroIndex++;
			
			if(gyro_sample[prev] >= 360 && randG > gyro_sample[prev])
				gyro_sample[gyroIndex] = 1;

			else if(gyro_sample[prev] <= 1 && randG < gyro_sample[prev])
				gyro_sample[gyroIndex] = 360;

			else if(randG > gyro_sample[prev])
				gyro_sample[gyroIndex] = gyro_sample[prev] + 5;

			else if(randG < gyro_sample[prev])
				gyro_sample[gyroIndex] = gyro_sample[prev] - 5;

			else 
				gyro_sample[gyroIndex] = gyro_sample[prev];
			
			rt_mutex_release(&mutex_gyro);
			
		}
		
		rt_mutex_acquire(&mutex_gyro,TM_INFINITE);
		gyroIndex = 0;
		rt_mutex_release(&mutex_gyro);
	}
}
Пример #19
0
void semWait1(){
	
	rt_mutex_acquire(&a, 0);
	rt_printf("Low pri task: lock a\n");
	//rt_timer_spin(3000000);
	busy_wait_ms(3);
	rt_mutex_acquire(&b, 0);
	rt_printf("Low pri task: lock b\n");
	busy_wait_ms(3);
	
	rt_printf("Low pri task: unlock b \n");
	rt_mutex_release(&b);
	rt_printf("Low pri task: unlock a \n");
	rt_mutex_release(&a);
	
	rt_printf("Low pri task: stop \n");
}
Пример #20
0
/* private functions ======================================================== */
static void task_soft_routine(void * cookie) {
    unsigned long n = 0; /* how many wheel rotations since init? */
    RTIME time_init = 0; /* when did we start? */
    RTIME time_prev = 0; /* when was the previous rotation? */
    RTIME time_curr = 0; /* when was the current rotation? */

    /* we start now! (first wheel rotation) */
    rt_queue_read(&queue, &time_init, sizeof time_curr, TM_INFINITE);
 
    /* previous rotation is now! (init value) */ 
    time_prev = time_init;

    while (1) {
        /* extract the current rotation timestamp from message queue */
        rt_queue_read(&queue, &time_curr, sizeof time_curr, TM_INFINITE);

        /* 
           Let's compute instant speed. We want an Hz value, we've got previous 
           rotation timestamp and current rotation timestamp, substracting them
           give us the time elapsed since previous rotation in nanoseconds. 
           Hz = 1 / s, so Hz = 10^9 / ns, our final value is 10^9 / dt
        */
        rt_mutex_acquire(&mutex_instant, TM_INFINITE);
        instant = 1e9 / (time_curr - time_prev);
        rt_mutex_release(&mutex_instant);

	/*
           Let's compute average speed. We want an Hz value, we've got init
           rotation timestamp and current rotation timestamp, substracting
           them gives us the time elapsed since init rotation in nanoseconds.
           ++n is the number of wheel rotations since init.
           Hz = 1 / s, so Hz = 10^9 / ns, our final value is 10^9 * dx / dt
        */
        rt_mutex_acquire(&mutex_average, TM_INFINITE);
        average = 1e9 * ++n / (time_curr - time_init);
        rt_mutex_release(&mutex_average);

        /* dump current timestamp to file */
        fprintf(ostream, "%llu\n", time_curr);

        /* our job is done, we are now the previous rotation */ 
        time_prev = time_curr;
    }

    (void) cookie;
}
Пример #21
0
void bouncingCheck (int x,int y, int radius, int directionX,int directionY){
        if(x + directionX < resolution_x){
            changeBallDirectionHorizontal();
        }
        else if(x +directionX + radius > resolution_w){
            changeBallDirectionHorizontal();
        }
        /* check if we hit the topmost pong */
        if (y+ directionY + (radius/4) <pongs[opponent].leftCorner.point_y + pongs[opponent].height ){
             if(x+directionX + radius > pongs[opponent].leftCorner.point_x &&
                x+directionX + radius < pongs[opponent].leftCorner.point_x+pongLength+radius){
                    bounceOpponent = true;
                    changeBallDirectionVertical();
                }
             else{ // in the vicinity to score a point 
                 player_score++;
		 rt_mutex_acquire(&txMutex[E_GAME],TM_INFINITE);
		 memset(gameData, 0, sizeof(gameData));
		 sprintf(gameData, "score;%d;%d;",player_score,opponent_score);
		 padString(gameData);
		 rt_mutex_release(&txMutex[E_GAME]);                 
                 reset = true;
             }
            
         }
	
        /* check if we hit the bottom-most pong */  
        if (y+ directionY + (radius/4) +pongs[player].height >pongs[player].leftCorner.point_y){
	     if(x+directionX + radius > pongs[player].leftCorner.point_x &&
                x+directionX + radius < pongs[player].leftCorner.point_x+pongLength+radius){
                    bounceplayer = true; 
                    changeBallDirectionVertical();
                }
	     else{                  opponent_score++;
                 rt_mutex_acquire(&txMutex[E_GAME],TM_INFINITE);
 		 memset(gameData, 0, sizeof(gameData));
		 sprintf(gameData, "score;%d;%d;",player_score,opponent_score);
		 padString(gameData);
		 rt_mutex_release(&txMutex[E_GAME]); 
                 reset = true;
             }
             
       }
		
}
Пример #22
0
/**
 * Get the elapsed time since the last alarm
 * @return a time in nanoseconds
 */
TIMEVAL getElapsedTime(void)
{
	RTIME res;
	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
	last_time_read = rt_timer_read();
	res = last_time_read - last_occured_alarm;
	rt_mutex_release(&condition_mutex);
	return res;
}
Пример #23
0
void lowFunc() {
	rt_sem_p(&sem, TM_INFINITE);
	rt_mutex_acquire(&resourceMutex, TM_INFINITE);
	rt_printf("low locks resource\n");
	busy_wait_ms(100);
	busy_wait_ms(100);
	busy_wait_ms(100);
	rt_printf("low unlocks resource\n");
	rt_mutex_release(&resourceMutex);
}
Пример #24
0
void communiquer(void *arg) {
    DMessage *msg = d_new_message();
    int var1 = 1;
    int num_msg = 0;
 
    rt_printf("tserver : Début de l'exécution de serveur\n");
    serveur->open(serveur, "8000");
    rt_printf("tserver : Connexion\n");
 
    rt_mutex_acquire(&mutexEtat, TM_INFINITE);
    etatCommMoniteur = 0;
    rt_mutex_release(&mutexEtat);
 
    while (var1 > 0) {
        rt_printf("tserver : Attente d'un message\n");
        var1 = serveur->receive(serveur, msg);
        num_msg++;
        if (var1 > 0) {
            switch (msg->get_type(msg)) {
                case MESSAGE_TYPE_ACTION:
                    rt_printf("tserver : Le message %d reçu est une action\n",
                            num_msg);
                    DAction *action = d_new_action();
                    action->from_message(action, msg);
                    switch (action->get_order(action)) {
                        case ACTION_CONNECT_ROBOT:
                            rt_printf("tserver : Action connecter robot\n");
                            rt_sem_v(&semConnecterRobot);
                            break;
                    }
                    break;
                case MESSAGE_TYPE_MOVEMENT:
                    rt_printf("tserver : Le message reçu %d est un mouvement\n",
                            num_msg);
                    rt_mutex_acquire(&mutexMove, TM_INFINITE);
                    move->from_message(move, msg);
                    move->print(move);
                    rt_mutex_release(&mutexMove);
                    break;
            }
        }
    }
}
Пример #25
0
void ElevatorController::updateStatusBuffer()
{
	//upheap and the downheap (hallcall and floor selections should benotifyDirectionChanged included.
	rt_mutex_acquire(&(this->rtData.mutexBuffer), TM_INFINITE);
	unsigned char selectedBuffer = this->eStat.bufferSelection;
	rt_mutex_release(&(this->rtData.mutexBuffer));
	
	rt_mutex_acquire(&(this->rtData.mutex), TM_INFINITE);
	this->eStat.statusBuffer[selectedBuffer][STATUS_CURRENT_FLOOR_INDEX] = this->eStat.getCurrentFloor();
	this->eStat.statusBuffer[selectedBuffer][STATUS_DIRECTION_INDEX] = this->eStat.getDirection();
	this->eStat.statusBuffer[selectedBuffer][STATUS_CURRENT_POSITION_INDEX] = this->eStat.getCurrentPosition();
	this->eStat.statusBuffer[selectedBuffer][STATUS_CURRENT_SPEED_INDEX] = this->eStat.getCurrentSpeed();
	//rt_printf("writting to buffer %d %s\n", selectedBuffer, statusBuffer[selectedBuffer]);
	rt_mutex_release(&(this->rtData.mutex));

	rt_mutex_acquire(&(this->rtData.mutexBuffer), TM_INFINITE);
	this->eStat.bufferSelection = ++(this->eStat.bufferSelection) % 2;
	rt_mutex_release(&(this->rtData.mutexBuffer));
}
Пример #26
0
void highFunc() {
	rt_sem_p(&sem, TM_INFINITE);
	rt_task_sleep_ms(200);
	rt_printf("High wants to lock resource\n");
	rt_mutex_acquire(&resourceMutex, TM_INFINITE);
	rt_printf("High locks resource\n");
	busy_wait_ms(100);
	busy_wait_ms(100);
	rt_printf("High unlocks resource\n");
	rt_mutex_release(&resourceMutex);
}
Пример #27
0
void resetScore(){
        if(nPongs >= MAX_PONGS){
            player_score = 0;
            opponent_score = 0;
            rt_mutex_acquire(&txMutex[E_GAME], TM_INFINITE);
	    memset(gameData, 0, sizeof(gameData));
  	    sprintf(gameData, "reset;");
	    padString(gameData);
	    rt_mutex_release(&txMutex[E_GAME]);
        }
} 
Пример #28
0
void sensorTask(long arg)
{
   	while(1){
		int i = 0;
		rt_mutex_acquire(&mutex_donnee,TM_INFINITE);		
		printf("******************** SENSOR TASK ********************\n");
		while(tabDonnePris==1){
			rt_mutex_release(&mutex_donnee);
			 rt_sem_p(&sem_donnee,TM_INFINITE);
			 rt_mutex_acquire(&mutex_donnee,TM_INFINITE);
		}
		tabDonnePris==1;
		for(i = 0; i<SENSOR_SIZE; i++){
			sensorArray[i] = rand()%16;
		}
		tabDonnePris = 0;
		rt_mutex_release(&mutex_donnee);
		usleep(100000);
	}
}
Пример #29
0
void Verif_Comm (int status) { 
    int compteur;
    DMessage *message;
    if (status != STATUS_OK) {
        rt_mutex_acquire(&mutexCpt, TM_INFINITE);
        cpt_perte_com ++;
        compteur = cpt_perte_com;
        rt_mutex_release(&mutexCpt);
         
        if (compteur >= 6) {
             
            // La communication est perdue
             
            rt_mutex_acquire(&mutexEtat, TM_INFINITE);
            etatCommRobot = status;
            rt_mutex_release(&mutexEtat);
             
            message = d_new_message();
            message->put_state(message, status);
            rt_printf("tmove : Envoi message\n");
            if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
                message->free(message);
            }
            
            //rt_sem_v(&semConnectedRobot);
             
            // On réinitialise le compteur
            rt_mutex_acquire(&mutexCpt, TM_INFINITE);
            cpt_perte_com = 0 ;
            rt_mutex_release(&mutexCpt);
            compteur = 0 ;
             
            // On reset le robot
             
            //rt_mutex_acquire(&mutexRobot, TM_INFINITE);
            robot->close_com(robot); // a verifier
           // rt_mutex_release(&mutexRobot);
             
        }
    }
}
Пример #30
0
void ElevatorController::floorRun() {
	unsigned char topItem;
	while(true)
	{
		if(!this->eStat.getGDFailedEmptyHeap())
		{
			rt_mutex_acquire(&(this->rtData.mutex), TM_INFINITE);
			rt_cond_wait(&(this->rtData.freeCond), &(this->rtData.mutex), TM_INFINITE);

			int upHeapSize = this->getUpHeap().getSize();
			int downHeapSize = this->getDownHeap().getSize();
			if(upHeapSize==0 && downHeapSize==0){this->eStat.setGDFailedEmptyHeap(true);}

			if(this->eStat.getServiceDirection() == DIRECTION_DOWN)
			{
				if(downHeapSize > 0)
				{
					topItem = this->getDownHeap().peek();
				}else if(upHeapSize > 0)
				{
					topItem = this->getUpHeap().peek();
				}this->downHeap.pushFloorRequestVector(this->missedFloorSelections);
			}else if(this->eStat.getServiceDirection() == DIRECTION_UP)
			{
				if(upHeapSize > 0)
				{
					topItem = this->getUpHeap().peek();
				}else if(downHeapSize > 0)
				{
					topItem = this->getDownHeap().peek();
				}
			}
		
			int tempDirection = this->eStat.getDirection();
			if(topItem != this->eStat.getDestination())
			{
				rt_printf("FR%d next Dest is %d\n.", this->getID(), topItem);
				this->getSimulator()->setFinalDestination(topItem);
				this->eStat.setDestination(topItem);
				rt_printf("FR%d tempDirection is %d afterDirection is %d\n.", this->getID(), tempDirection, this->getSimulator()->getDirection());
			}

			if(tempDirection != this->getSimulator()->getDirection())
			{
				this->updateMissedFloor(tempDirection);
				this->notifyDirectionChanged(this->getSimulator()->getDirection());
			}

			rt_mutex_release(&(this->rtData.mutex));
		}
	}
}