|
@@ -276,31 +276,53 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
276
|
276
|
/**************************************************************************************/
|
277
|
277
|
/* The task receiveFromMon starts here */
|
278
|
278
|
/**************************************************************************************/
|
|
279
|
+ while(1){
|
279
|
280
|
rt_sem_p(&sem_serverOk, TM_INFINITE);
|
280
|
281
|
cout << "Received message from monitor activated" << endl << flush;
|
281
|
282
|
|
282
|
|
- while (1) {
|
283
|
|
- msgRcv = monitor.Read();
|
284
|
|
- cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
|
285
|
|
-
|
286
|
|
- if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
|
287
|
|
- delete(msgRcv);
|
288
|
|
- exit(-1);
|
289
|
|
- } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
|
290
|
|
- rt_sem_v(&sem_openComRobot);
|
291
|
|
- } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
|
292
|
|
- rt_sem_v(&sem_startRobot);
|
293
|
|
- } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
|
294
|
|
- msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
|
295
|
|
- msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
|
296
|
|
- msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
|
297
|
|
- msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
|
|
283
|
+ while (1) {
|
|
284
|
+ msgRcv = monitor.Read();
|
|
285
|
+ cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
|
|
286
|
+
|
|
287
|
+ if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
|
|
288
|
+ delete(msgRcv);
|
|
289
|
+ WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
|
|
290
|
+ WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET));
|
|
291
|
+ WriteInQueue(&q_messageControlCam, new Message(MESSAGE_CAM_CLOSE));
|
|
292
|
+ rt_sem_v(&sem_restartServer, TM_INFINITE); // A VERIFIER
|
|
293
|
+ break;
|
|
294
|
+
|
|
295
|
+ } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) {
|
|
296
|
+ WriteInQueue(&q_messageComRobot, msgRcv);
|
|
297
|
+ //rt_sem_v(&sem_openComRobot);
|
|
298
|
+ } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD) || msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
|
|
299
|
+ WriteInQueue(&q_messageControlRobot, msgRcv);
|
|
300
|
+ //rt_sem_v(&sem_startRobot);
|
|
301
|
+ }
|
|
302
|
+ else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
|
|
303
|
+ msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
|
|
304
|
+ msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
|
|
305
|
+ msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
|
|
306
|
+ msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
|
|
307
|
+ rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
|
308
|
+ move = msgRcv->GetID();
|
|
309
|
+ rt_mutex_release(&mutex_move);
|
|
310
|
+ }
|
298
|
311
|
|
299
|
|
- rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
300
|
|
- move = msgRcv->GetID();
|
301
|
|
- rt_mutex_release(&mutex_move);
|
|
312
|
+ else if (msgRcv->CompareID(MESSAGE_CAM_OPEN) ||
|
|
313
|
+ msgRcv->CompareID(MESSAGE_CAM_CLOSE) ||
|
|
314
|
+ msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA) ||
|
|
315
|
+ msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) ||
|
|
316
|
+ msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) ||
|
|
317
|
+ msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) ||
|
|
318
|
+ msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP) ||
|
|
319
|
+ )
|
|
320
|
+ {
|
|
321
|
+ WriteInQueue(&q_messageControlCam, msgRcv);
|
|
322
|
+ }
|
|
323
|
+ delete(msgRcv); // mus be deleted manually, no consumer
|
302
|
324
|
}
|
303
|
|
- delete(msgRcv); // mus be deleted manually, no consumer
|
|
325
|
+
|
304
|
326
|
}
|
305
|
327
|
}
|
306
|
328
|
|
|
@@ -350,23 +372,70 @@ void Tasks::StartRobotTask(void *arg) {
|
350
|
372
|
/**************************************************************************************/
|
351
|
373
|
while (1) {
|
352
|
374
|
|
|
375
|
+ Message * tmp;
|
353
|
376
|
Message * msgSend;
|
354
|
|
- rt_sem_p(&sem_startRobot, TM_INFINITE);
|
355
|
|
- cout << "Start robot without watchdog (";
|
356
|
|
- rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
357
|
|
- msgSend = robot.Write(robot.StartWithoutWD());
|
358
|
|
- rt_mutex_release(&mutex_robot);
|
359
|
|
- cout << msgSend->GetID();
|
360
|
|
- cout << ")" << endl;
|
361
|
|
-
|
362
|
|
- cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
363
|
|
- WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
364
|
|
-
|
365
|
|
- if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
|
|
377
|
+ tmp = ReadInQueue(&q_messageControlRobot);
|
|
378
|
+ if (tmp -> CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)){
|
|
379
|
+ cout << "Start robot without watchdog (";
|
|
380
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
381
|
+ msgSend = robot.Write(robot.StartWithoutWD());
|
|
382
|
+ rt_mutex_release(&mutex_robot);
|
|
383
|
+ cout << msgSend->GetID();
|
|
384
|
+ cout << ")" << endl;
|
|
385
|
+ cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
|
386
|
+ WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
|
387
|
+ if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
|
|
388
|
+ rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
|
389
|
+ robotStarted = 1;
|
|
390
|
+ rt_mutex_release(&mutex_robotStarted);
|
|
391
|
+ }
|
|
392
|
+ } else if (tmp -> CompareID(MESSAGE_ROBOT_START_WITH_WD)){
|
|
393
|
+ cout << "Start robot with watchdog (";
|
|
394
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
395
|
+ msgSend = robot.Write(robot.StartWithWD());
|
|
396
|
+ rt_mutex_release(&mutex_robot);
|
|
397
|
+ cout << msgSend->GetID();
|
|
398
|
+ cout << ")" << endl;
|
|
399
|
+ cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
|
400
|
+ WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
|
401
|
+ if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
|
|
402
|
+ rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
|
403
|
+ robotStarted = 1;
|
|
404
|
+ rt_mutex_release(&mutex_robotStarted);
|
|
405
|
+ rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
|
|
406
|
+ }
|
|
407
|
+
|
|
408
|
+ } else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){
|
|
409
|
+ rt_task_set_periodic(&th_watchDog,TM_NOW,0);
|
|
410
|
+ cout << "Stopping Robot (";
|
|
411
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
412
|
+ msgSend = robot.Write(new Message(MESSAGE_ROBOT_RESET));
|
|
413
|
+ rt_mutex_release(&mutex_robot);
|
|
414
|
+ cout << msgSend->GetID();
|
|
415
|
+ cout << ")" << endl;
|
|
416
|
+ cout << "Movement answer: " << msgSend->ToString() << endl << flush;
|
|
417
|
+ WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
|
366
|
418
|
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
367
|
|
- robotStarted = 1;
|
|
419
|
+ robotStarted = 0;
|
368
|
420
|
rt_mutex_release(&mutex_robotStarted);
|
369
|
|
- }
|
|
421
|
+ }
|
|
422
|
+
|
|
423
|
+
|
|
424
|
+ }
|
|
425
|
+}
|
|
426
|
+
|
|
427
|
+
|
|
428
|
+void Tasks::WatchDog(void *arg) {
|
|
429
|
+ cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
|
430
|
+ // Synchronization barrier (waiting that all tasks are starting)
|
|
431
|
+ rt_sem_p(&sem_barrier, TM_INFINITE);
|
|
432
|
+ rt_task_set_periodic(NULL, TM_NOW, 0);
|
|
433
|
+ while (1) {
|
|
434
|
+ rt_task_wait_period(NULL);
|
|
435
|
+ Message * msgSend;
|
|
436
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
437
|
+ msgSend = robot.ReloadWD();
|
|
438
|
+ rt_mutex_release(&mutex_robot);
|
370
|
439
|
}
|
371
|
440
|
}
|
372
|
441
|
|
|
@@ -403,7 +472,6 @@ void Tasks::MoveTask(void *arg) {
|
403
|
472
|
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
404
|
473
|
robot.Write(new Message((MessageID)cpMove));
|
405
|
474
|
rt_mutex_release(&mutex_robot);
|
406
|
|
-
|
407
|
475
|
previousMove = cpMove;
|
408
|
476
|
}
|
409
|
477
|
}
|