|
@@ -285,32 +285,53 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
285
|
285
|
/**************************************************************************************/
|
286
|
286
|
/* The task receiveFromMon starts here */
|
287
|
287
|
/**************************************************************************************/
|
|
288
|
+ while(1){
|
288
|
289
|
rt_sem_p(&sem_serverOk, TM_INFINITE);
|
289
|
290
|
cout << "Received message from monitor activated" << endl << flush;
|
290
|
291
|
|
291
|
|
- while (1) {
|
292
|
|
- msgRcv = monitor.Read();
|
293
|
|
- cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
|
294
|
|
-
|
295
|
|
- if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
|
296
|
|
- delete(msgRcv);
|
297
|
|
- exit(-1);
|
298
|
|
- } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
|
299
|
|
- //rt_sem_v(&sem_openComRobot);
|
300
|
|
- WriteInQueue(&q_messageComRobot, msgRcv);
|
301
|
|
- } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
|
302
|
|
- rt_sem_v(&sem_startRobot);
|
303
|
|
- } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
|
304
|
|
- msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
|
305
|
|
- msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
|
306
|
|
- msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
|
307
|
|
- msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
|
|
292
|
+ while (1) {
|
|
293
|
+ msgRcv = monitor.Read();
|
|
294
|
+ cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
|
|
295
|
+
|
|
296
|
+ if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
|
|
297
|
+ delete(msgRcv);
|
|
298
|
+ WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
|
|
299
|
+ WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET));
|
|
300
|
+ WriteInQueue(&q_messageControlCam, new Message(MESSAGE_CAM_CLOSE));
|
|
301
|
+ rt_sem_v(&sem_restartServer, TM_INFINITE); // A VERIFIER
|
|
302
|
+ break;
|
|
303
|
+
|
|
304
|
+ } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) {
|
|
305
|
+ WriteInQueue(&q_messageComRobot, msgRcv);
|
|
306
|
+ //rt_sem_v(&sem_openComRobot);
|
|
307
|
+ } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD) || msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
|
|
308
|
+ WriteInQueue(&q_messageControlRobot, msgRcv);
|
|
309
|
+ //rt_sem_v(&sem_startRobot);
|
|
310
|
+ }
|
|
311
|
+ else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
|
|
312
|
+ msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
|
|
313
|
+ msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
|
|
314
|
+ msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
|
|
315
|
+ msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
|
|
316
|
+ rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
|
317
|
+ move = msgRcv->GetID();
|
|
318
|
+ rt_mutex_release(&mutex_move);
|
|
319
|
+ }
|
308
|
320
|
|
309
|
|
- rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
310
|
|
- move = msgRcv->GetID();
|
311
|
|
- rt_mutex_release(&mutex_move);
|
|
321
|
+ else if (msgRcv->CompareID(MESSAGE_CAM_OPEN) ||
|
|
322
|
+ msgRcv->CompareID(MESSAGE_CAM_CLOSE) ||
|
|
323
|
+ msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA) ||
|
|
324
|
+ msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) ||
|
|
325
|
+ msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) ||
|
|
326
|
+ msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) ||
|
|
327
|
+ msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP) ||
|
|
328
|
+ )
|
|
329
|
+ {
|
|
330
|
+ WriteInQueue(&q_messageControlCam, msgRcv);
|
|
331
|
+ }
|
|
332
|
+ delete(msgRcv); // mus be deleted manually, no consumer
|
312
|
333
|
}
|
313
|
|
- delete(msgRcv); // mus be deleted manually, no consumer
|
|
334
|
+
|
314
|
335
|
}
|
315
|
336
|
}
|
316
|
337
|
|
|
@@ -381,23 +402,70 @@ void Tasks::StartRobotTask(void *arg) {
|
381
|
402
|
/**************************************************************************************/
|
382
|
403
|
while (1) {
|
383
|
404
|
|
|
405
|
+ Message * tmp;
|
384
|
406
|
Message * msgSend;
|
385
|
|
- rt_sem_p(&sem_startRobot, TM_INFINITE);
|
386
|
|
- cout << "Start robot without watchdog (";
|
387
|
|
- rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
388
|
|
- msgSend = robot.Write(robot.StartWithoutWD());
|
389
|
|
- rt_mutex_release(&mutex_robot);
|
390
|
|
- cout << msgSend->GetID();
|
391
|
|
- cout << ")" << endl;
|
392
|
|
-
|
393
|
|
- cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
394
|
|
- WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
395
|
|
-
|
396
|
|
- if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
|
|
407
|
+ tmp = ReadInQueue(&q_messageControlRobot);
|
|
408
|
+ if (tmp -> CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)){
|
|
409
|
+ cout << "Start robot without watchdog (";
|
|
410
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
411
|
+ msgSend = robot.Write(robot.StartWithoutWD());
|
|
412
|
+ rt_mutex_release(&mutex_robot);
|
|
413
|
+ cout << msgSend->GetID();
|
|
414
|
+ cout << ")" << endl;
|
|
415
|
+ cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
|
416
|
+ WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
|
417
|
+ if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
|
|
418
|
+ rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
|
419
|
+ robotStarted = 1;
|
|
420
|
+ rt_mutex_release(&mutex_robotStarted);
|
|
421
|
+ }
|
|
422
|
+ } else if (tmp -> CompareID(MESSAGE_ROBOT_START_WITH_WD)){
|
|
423
|
+ cout << "Start robot with watchdog (";
|
|
424
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
425
|
+ msgSend = robot.Write(robot.StartWithWD());
|
|
426
|
+ rt_mutex_release(&mutex_robot);
|
|
427
|
+ cout << msgSend->GetID();
|
|
428
|
+ cout << ")" << endl;
|
|
429
|
+ cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
|
430
|
+ WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
|
431
|
+ if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
|
|
432
|
+ rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
|
433
|
+ robotStarted = 1;
|
|
434
|
+ rt_mutex_release(&mutex_robotStarted);
|
|
435
|
+ rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
|
|
436
|
+ }
|
|
437
|
+
|
|
438
|
+ } else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){
|
|
439
|
+ rt_task_set_periodic(&th_watchDog,TM_NOW,0);
|
|
440
|
+ cout << "Stopping Robot (";
|
|
441
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
442
|
+ msgSend = robot.Write(new Message(MESSAGE_ROBOT_RESET));
|
|
443
|
+ rt_mutex_release(&mutex_robot);
|
|
444
|
+ cout << msgSend->GetID();
|
|
445
|
+ cout << ")" << endl;
|
|
446
|
+ cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
|
447
|
+ WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
397
|
448
|
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
398
|
|
- robotStarted = 1;
|
|
449
|
+ robotStarted = 0;
|
399
|
450
|
rt_mutex_release(&mutex_robotStarted);
|
400
|
|
- }
|
|
451
|
+ }
|
|
452
|
+
|
|
453
|
+
|
|
454
|
+ }
|
|
455
|
+}
|
|
456
|
+
|
|
457
|
+
|
|
458
|
+void Tasks::WatchDog(void *arg) {
|
|
459
|
+ cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
|
460
|
+ // Synchronization barrier (waiting that all tasks are starting)
|
|
461
|
+ rt_sem_p(&sem_barrier, TM_INFINITE);
|
|
462
|
+ rt_task_set_periodic(NULL, TM_NOW, 0);
|
|
463
|
+ while (1) {
|
|
464
|
+ rt_task_wait_period(NULL);
|
|
465
|
+ Message * msgSend;
|
|
466
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
467
|
+ msgSend = robot.ReloadWD();
|
|
468
|
+ rt_mutex_release(&mutex_robot);
|
401
|
469
|
}
|
402
|
470
|
}
|
403
|
471
|
|
|
@@ -434,7 +502,6 @@ void Tasks::MoveTask(void *arg) {
|
434
|
502
|
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
435
|
503
|
robot.Write(new Message((MessageID)cpMove));
|
436
|
504
|
rt_mutex_release(&mutex_robot);
|
437
|
|
-
|
438
|
505
|
previousMove = cpMove;
|
439
|
506
|
}
|
440
|
507
|
}
|