void demo(void *arg)
{
//Round Robin
rt_task_set_mode(0,T_RRB,NULL);
rt_task_slice(NULL,15); //time slice in jiffies
RTIME starttime, runtime;
int num=*(int *)arg;
RT_TASK *curtask;
RT_TASK_INFO curtaskinfo;
rt_printf("Task : %d\n",num);
rt_sem_p(&mysync,TM_INFINITE);
// let the task run RUNTIME(=200) jiffies in steps of SPINTIME(=20) jiffies
runtime = 0;
while(runtime < EXECTIME) {
rt_timer_spin(SPINTIME*BASEPERIOD); // spin cpu doing nothing
// note: rt_timer_spin function does not accept jiffies only nanoseconds
// deviates from timing conventions throughout the Xenomai API
runtime = runtime + SPINTIME;
rt_printf("Running Task : %d at time : %d\n",num,runtime);
}
rt_printf("End Task : %d\n",num);
}
void verifier_batterie(void *arg) {
DBattery *batterie = d_new_battery();
int status;
int vbat;
rt_printf("tbattery : Attente du sémaphore semVerifierBatterie\n");
rt_sem_p(&semVerifierBatterie, TM_INFINITE);
rt_printf("tbattery : Debut de l'execution periodique à 1s\n");
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
while (1) {
while (cptCommErr < MAX_ECHECS) {
/* Attente de l'activation périodique */
rt_task_wait_period(NULL);
rt_printf("tbattery : Activation périodique\n");
status = robot->get_vbat(robot, &vbat);
if (status == STATUS_OK) {
cptCommErr = 0;
if (vbat == BATTERY_OFF || vbat == BATTERY_LOW || vbat == BATTERY_OK)
batterie->set_level(batterie, vbat);
} else {
cptCommErr++;
rt_printf("tbattery : Erreur de communication avec le robot (%d)\n", cptCommErr);
}
}
comm_err_handler(status);
rt_sem_p(&semVerifierBatterie, TM_INFINITE);
}
}
void recharger_watchdog(void *arg) {
int status;
rt_printf("twatchdog : Attente du sémaphore semRechargerWatchdog\n");
rt_sem_p(&semRechargerWatchdog, TM_INFINITE);
rt_printf("twatchdog : Debut de l'execution periodique à 1s\n");
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
while (1) {
while (cptCommErr < MAX_ECHECS) {
/* Attente de l'activation périodique */
rt_task_wait_period(NULL);
rt_printf("twatchdog : Activation périodique\n");
status = robot->reload_wdt(robot);
if (status == STATUS_OK) {
cptCommErr = 0;
} else {
cptCommErr++;
rt_printf("twatchdog : Erreur de communication avec le robot (%d)\n", cptCommErr);
}
}
comm_err_handler(status);
rt_sem_p(&semRechargerWatchdog, TM_INFINITE);
}
}
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 task_H(void* data)
{
rt_sem_p(&syncsem,TM_INFINITE);
rt_task_sleep_ms(1);
rt_sem_p(&semB,TM_INFINITE);
rt_printf("H : I got B\n");
busy_wait_ms(1);
rt_sem_p(&semA,TM_INFINITE);
busy_wait_ms(2);
print_pri(NULL,(char) data);
rt_sem_v(&semB);
rt_sem_v(&semA);
}
void high(){
rt_sem_p(&synca, TM_INFINITE);
rt_task_sleep_ms(200);
rt_printf("HIGH RUNNING\n");
rt_sem_p(&semaphore, TM_INFINITE);
//rt_mutex_acquire(&mutex, TM_INFINITE);
rt_printf("HIGH AQUIRED LCOK\n");
busy_wait_ms(100);
busy_wait_ms(100);
rt_printf("HIGH FINISHED\n");
rt_sem_v(&semaphore);
//rt_mutex_release(&mutex);
}
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 task_L(void* data)
{
rt_sem_p(&syncsem,TM_INFINITE);
rt_sem_p(&semA,TM_INFINITE);
int prio = rt_task_set_priority(NULL,99);
rt_printf("L : I got A\n");
busy_wait_ms(3);
rt_sem_p(&semB,TM_INFINITE);
busy_wait_ms(3);
print_pri(NULL,(char) data);
rt_sem_v(&semB);
rt_sem_v(&semA);
}
void low(){
rt_sem_p(&synca, TM_INFINITE);
rt_printf("LOW RUNNING\n");
rt_sem_p(&semaphore, TM_INFINITE);
//rt_mutex_acquire(&mutex, TM_INFINITE);
rt_printf("LOW AQUIRED LOCK\n");
busy_wait_ms(100);
busy_wait_ms(100);
busy_wait_ms(100);
rt_printf("LOW FINISHED\n");
rt_sem_v(&semaphore);
//rt_mutex_release(&mutex);
}
static void sync_threads(int id){
if(rt_sem_p(&sync_sem, TM_INFINITE) == SUCCESS){
rt_printf("Task %i got semaphore\n", id);
}else{
rt_printf("Task %i failed to get semaphore \n", id);
}
}
void demo(void *arg)
{
RTIME starttime, runtime;
int num=*(int *)arg;
RT_TASK *curtask;
RT_TASK_INFO curtaskinfo;
curtask=rt_task_self();
rt_printf("Task : %d\n",num);
rt_sem_p(&mysync,TM_INFINITE);
runtime = 0;
while(runtime < EXECTIME) {
rt_timer_spin(SPINTIME); // spin cpu doing nothing
runtime = runtime + SPINTIME;
rt_printf("Running Task : %d at ms : %d\n",num,runtime/1000000);
if(runtime == (EXECTIME/2)){
if(num == 2){
rt_task_set_priority(&demo_task[1],MID+10);
rt_task_set_priority(&demo_task[0],LOW+10);
}
}
}
rt_printf("End Task : %d\n",num);
}
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);
}
}
void prioHigh(void *arg){
int err = 0;
rt_task_sleep(WAIT);
int i = 0;
while(i<3) {
rt_printf("High priority task tries to lock semaphore\n");
err = rt_sem_p(&mysync,TM_INFINITE);
if(err < 0) rt_printf("Failed pending semaphore; error: %d: %s", err, strerror(-err));
err = 0;
rt_printf("High priority task locks semaphore\n");
rt_timer_spin(SPINTIME); // spin cpu doing nothing
i++;
rt_printf("High priority task unlocks semaphore\n");
err = rt_sem_v(&mysync);
if(err < 0) rt_printf("Failed signaling semaphore; error: %d: %s", err, strerror(-err));
err = 0;
}
rt_printf("..........................................High priority task ends\n");
}
void taskOne(void *arg){
int i;
for(i=0;i<ITER;i++){
rt_sem_p(&semGlobal1,0);
rt_printf("I'm taskOne and global = %d...\n",++global);
rt_sem_v(&semGlobal2);
}
}
void taskTwo(void *arg){
int i;
for(i=0;i<ITER;i++){
rt_sem_p(&semGlobal2,0);
rt_printf("I'm taskTwo and global = %d---\n",--global);
rt_sem_v(&semGlobal1);
}
}
void task_M(void* data)
{
rt_sem_p(&syncsem,TM_INFINITE);
rt_task_sleep_ms(1);
//rt_sem_p(sem,TM_INFINITE);
busy_wait_ms(5);
print_pri(NULL,(char) data);
//rt_sem_v(sem,TM_INFINITE);
}
void taskM ( void ) {
rt_sem_p(&xenomai_semaphore, TM_INFINITE); // wait for synch
print_pri(&taskMed, "TaskM started\n");
rt_task_sleep(TIME_UNIT);
print_pri(&taskMed, "TaskM starting work\n");
busy_wait_ms(5*TIME_UNIT_MS);
print_pri(&taskMed, "TaskM complete\n");
}
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 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 medFunc() {
rt_sem_p(&sem, TM_INFINITE);
rt_task_sleep_ms(100);
rt_printf("med starts running\n");
busy_wait_ms(100);
busy_wait_ms(100);
busy_wait_ms(100);
busy_wait_ms(100);
busy_wait_ms(100);
rt_printf("med finished\n");
}
void medium(){
rt_sem_p(&synca, TM_INFINITE);
rt_task_sleep_ms(100);
rt_printf("MEDIUM RUNNING\n");
busy_wait_ms(100);
busy_wait_ms(100);
busy_wait_ms(100);
busy_wait_ms(100);
busy_wait_ms(100);
rt_printf("MEDIUM FINISHED\n");
}
void demo(void *arg)
{
int num = * (int *)arg;
RT_TASK *curtask;
RT_TASK_INFO curtaskinfo;
curtask=rt_task_self();
rt_task_inquire(curtask, &curtaskinfo);
rt_sem_p(&sem, TM_INFINITE);
rt_printf("Task name: %s - Argument %d\n", curtaskinfo.name, num);
rt_sem_v(&sem);
}
void executor(void *args){
rt_task_set_periodic(NULL,TM_NOW,PLCperiod);
short step = (short)*args;
actuators = 0; // how to initialize the temporary value of actuators?
while(!stopped){
rt_sem_p(&readDone,0);
int c;
for(c=0;c<nStep;c++){
if(stepStatus[c])
step[c](sensors); // or equivalent
}
rt_sem_v(&executionDone);
rt_sem_p(&writeDone,0);
for(c=0;c<nStep;c++){
if(stepStatus[c])
condition[c](sensor,actuators);
}
rt_task_wait_period(NULL);
}
return;
}
void act(void *args){
rt_task_set_periodic(NULL,TM_NOW,PLCperiod);
while(!stopped){
rt_sem_p(&executionDone,0);
/*
* Write outputs status
*/
writeOutputs(actuators);
rt_sem_v(&writeDone);
rt_task_wait_period(NULL);
}
}
void printTask(long arg){
usleep(2);
char buffer[50];
char* string;
while(1){
rt_sem_p(&sem_obs,TM_INFINITE);
rt_mutex_acquire(&mutex_obs,TM_INFINITE);
printf("******************** PRINT TASK ********************\n");
tabObstaclePris=1;
obstacle* temp = obstacleList;
while(temp->nxt != NULL){
printf("Obstacle found ! Distance : %d\n",temp->distance);
temp = temp->nxt;
}
tabObstaclePris = 0;
temp= obstacleList;
temp->nxt=NULL;
temp->distance=0;
rt_mutex_release(&mutex_obs);
//Fin de la simulation, calcul du temps d'exécution
gettimeofday(&tim, NULL);
stop = tim.tv_sec+(tim.tv_usec/1000000.0);
elapsed = stop-start;
/********** Ecriture dans le fichier **********/
file = fopen("output", "a");
string = "Lap time : ";
sprintf(buffer, "%lf", elapsed);
fputs(string, file);
fputs(buffer, file);
fputs("\n", file);
fclose(file);
/********** Fin ecriture dans le fichier **********/
printf("Time : %lf\n", elapsed);
gettimeofday(&tim, NULL);
start = tim.tv_sec+(tim.tv_usec/1000000.0);
}
}
void taskH ( void ) {
rt_sem_p(&xenomai_semaphore, TM_INFINITE); // wait for synch
print_pri(&taskHigh, "TaskH started\n");
rt_task_sleep(2*TIME_UNIT);
rt_mutex_acquire(&resMut,NULL); // lock resource
print_pri(&taskHigh, "TaskH locked resource\n");
rt_task_sleep(2*TIME_UNIT);
rt_mutex_release(&resMut); // unlock the resource
print_pri(&taskHigh, "TaskH released resource\n");
}
void taskL ( void ) {
rt_sem_p(&xenomai_semaphore, TM_INFINITE); // wait for synch
print_pri(&taskLow, "TaskL started\n");
rt_mutex_acquire(&resMut,NULL); // lock resource
print_pri(&taskLow, "TaskL locked resource\n");
busy_wait_ms(3*TIME_UNIT_MS);
print_pri(&taskLow, "TaskL completed work\n");
print_pri(&taskLow, "TaskL released resource\n");
rt_mutex_release(&resMut); // release resource
}
void task(void *arg)
{
rt_sem_p(&semGlobal, TM_INFINITE);
int a = * (int *) arg;
RT_TASK *curtask;
RT_TASK_INFO curtaskinfo;
curtask=rt_task_self();
rt_task_inquire(curtask,&curtaskinfo);
rt_printf("Task name: %s arg: %d \n", curtaskinfo.name, a);
}
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);
}
}
}
}
static void test_task(void *arg)
{
int ret;
traceobj_enter(&trobj);
traceobj_mark(&trobj, 6);
ret = rt_sem_p(&sem, TM_INFINITE);
traceobj_check(&trobj, ret, 0);
traceobj_mark(&trobj, 7);
traceobj_exit(&trobj);
}
