Leonie Gallois 3 years ago
parent
commit
4ac28d8427

+ 103
- 36
software/raspberry/superviseur-robot/tasks.cpp View File

@@ -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
         }

+ 8
- 2
software/raspberry/superviseur-robot/tasks.h View File

@@ -99,6 +99,8 @@ private:
99 99
     RT_SEM sem_openComRobot;
100 100
     RT_SEM sem_serverOk;
101 101
     RT_SEM sem_startRobot;
102
+    RT_SEM sem_watchdogStart;
103
+    RT_SEM sem_restartServer;
102 104
     
103 105
     
104 106
 
@@ -107,7 +109,6 @@ private:
107 109
     /**********************************************************************/
108 110
     int MSG_QUEUE_SIZE;
109 111
     RT_QUEUE q_messageToMon;
110
-    
111 112
     RT_QUEUE q_messageComRobot;
112 113
     RT_QUEUE q_messageControlRobot;
113 114
     RT_QUEUE q_messageControlCam;
@@ -168,7 +169,12 @@ private:
168 169
      * @return Nothing
169 170
      */
170 171
     void ReadBattery(void *arg);
171
-
172
+    
173
+    /**
174
+     * Sends periodically a message to the robot
175
+     * @return Nothing
176
+     */
177
+    void Watchdog(void *arg);
172 178
 
173 179
 };
174 180
 

Loading…
Cancel
Save