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;
}
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++;
}
}
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");
}
}
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);
}
}
}
}
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);
}
}
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);
}
}
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);
}
}
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;
}
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]);
}
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);
}
/** 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;
}
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;
}
}
/**
* 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);
}
/** 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;
}
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]);
}
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;
}
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);
}
}
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");
}
/* 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;
}
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;
}
}
}
/**
* 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;
}
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);
}
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;
}
}
}
}
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));
}
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);
}
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]);
}
}
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);
}
}
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);
}
}
}
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));
}
}
}