Browse Source

ajout des taches vison et batterie

Romain Vitrat 4 years ago
parent
commit
673cd3390c

+ 174
- 193
software/raspberry/superviseur-robot/tasks.cpp View File

@@ -24,10 +24,10 @@
24 24
 #define PRIORITY_TMOVE 20
25 25
 #define PRIORITY_TSENDTOMON 22
26 26
 #define PRIORITY_TRECEIVEFROMMON 25
27
-#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22
28
-#define PRIORITY_TSTARTROBOTWITHWATCHDOG 22
29
-#define PRIORITY_TCAMERA 21
30
-
27
+#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 24
28
+#define PRIORITY_TSTARTROBOTWITHWATCHDOG 23
29
+#define PRIORITY_TCAMERA 21 
30
+#define PRIORITY_TVISION 26 
31 31
 
32 32
 /*
33 33
  * Some remarks:
@@ -75,55 +75,30 @@ void Tasks::Init() {
75 75
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
76 76
         exit(EXIT_FAILURE);
77 77
     }
78
-    if (err = rt_mutex_create(&mutex_killReceiveFromMon, NULL)) {
78
+    if (err = rt_mutex_create(&mutex_killBattery, NULL)) {
79 79
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
80 80
         exit(EXIT_FAILURE);
81 81
     }
82
-    
83 82
     if (err = rt_mutex_create(&mutex_killVision, NULL)) {
84 83
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
85 84
         exit(EXIT_FAILURE);
86 85
     }
87
-    
86
+    if (err = rt_mutex_create(&mutex_acquireImage, NULL)) {
87
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
88
+        exit(EXIT_FAILURE);
89
+    }
88 90
     if (err = rt_mutex_create(&mutex_searchArena, NULL)) {
89 91
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
90 92
         exit(EXIT_FAILURE);
91 93
     }
92
-
93 94
     if (err = rt_mutex_create(&mutex_drawArena, NULL)) {
94 95
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
95 96
         exit(EXIT_FAILURE);
96 97
     }
97
-    
98 98
     if (err = rt_mutex_create(&mutex_getPosition, NULL)) {
99 99
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
100 100
         exit(EXIT_FAILURE);
101 101
     }
102
-
103
-    if (err = rt_mutex_create(&mutex_killSendMon, NULL)) {
104
-        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
105
-        exit(EXIT_FAILURE);
106
-    }    
107
-
108
-    if (err = rt_mutex_create(&mutex_killDetectLostSupRob, NULL)) {
109
-        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
110
-        exit(EXIT_FAILURE);
111
-    } 
112
-    
113
-    if (err = rt_mutex_create(&mutex_acquireImage, NULL)) {
114
-        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
115
-        exit(EXIT_FAILURE);
116
-    }     
117
-
118
-    
119
-    
120
-    
121
-    /*if (err = rt_mutex_create(&mutex_robot_on, NULL)) {
122
-        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
123
-        exit(EXIT_FAILURE);
124
-    }*/
125
-    
126
-    
127 102
     cout << "Mutexes created successfully" << endl << flush;
128 103
 
129 104
     /**************************************************************************************/
@@ -149,16 +124,14 @@ void Tasks::Init() {
149 124
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
150 125
         exit(EXIT_FAILURE);
151 126
     }
152
-
153
-    if (err = rt_sem_create(&sem_allowStartReceive, NULL, 0, S_FIFO)) {
127
+    if (err = rt_sem_create(&sem_askBattery, NULL, 0, S_FIFO)) {
154 128
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
155 129
         exit(EXIT_FAILURE);
156
-    }   
157
-    
158
-    
159
-    //Initialization for some specific sem: Allow to run the first time
160
-    rt_sem_v(&sem_allowStartReceive);
161
-    
130
+    }
131
+    if (err = rt_sem_create(&sem_allowStartCaptureImage, NULL, 0, S_FIFO)) {
132
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
133
+        exit(EXIT_FAILURE);
134
+    }
162 135
     cout << "Semaphores created successfully" << endl << flush;
163 136
 
164 137
     /**************************************************************************************/
@@ -192,7 +165,14 @@ void Tasks::Init() {
192 165
         cerr << "Error task create: " << strerror(-err) << endl << flush;
193 166
         exit(EXIT_FAILURE);
194 167
     }
195
-    
168
+    if (err = rt_task_create(&th_askBattery, "th_askBattery", 0, PRIORITY_TMOVE, 0)) {
169
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
170
+        exit(EXIT_FAILURE);
171
+    }
172
+   /* if (err = rt_task_create(&th_vision, "th_vision", 0, PRIORITY_TVISION, 0)) {
173
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
174
+        exit(EXIT_FAILURE);
175
+    }*/
196 176
     cout << "Tasks created successfully" << endl << flush;
197 177
 
198 178
     /**************************************************************************************/
@@ -241,7 +221,14 @@ void Tasks::Run() {
241 221
         cerr << "Error task start: " << strerror(-err) << endl << flush;
242 222
         exit(EXIT_FAILURE);
243 223
     }
244
-
224
+    if (err = rt_task_start(&th_askBattery, (void(*)(void*)) & Tasks::AskBattery, this)) {
225
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
226
+        exit(EXIT_FAILURE);
227
+    }
228
+   /* if (err = rt_task_start(&th_vision, (void(*)(void*)) & Tasks::Vision, this)) {
229
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
230
+        exit(EXIT_FAILURE);
231
+    }*/
245 232
     cout << "Tasks launched" << endl << flush;
246 233
 }
247 234
 
@@ -318,119 +305,30 @@ void Tasks::SendToMonTask(void* arg) {
318 305
  */
319 306
 void Tasks::ReceiveFromMonTask(void *arg) {
320 307
     Message *msgRcv;
321
-    bool killReceiveFromMonOk=0;
322 308
     
323 309
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
324 310
     // Synchronization barrier (waiting that all tasks are starting)
325 311
     rt_sem_p(&sem_barrier, TM_INFINITE);
326 312
     
327
-    //Wait twin task to die if not first round
328
-    rt_sem_p(&sem_allowStartReceive, TM_INFINITE);
329
-    
330
-    //Reinitialize control boolean 
331
-    rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
332
-        killReceiveFromMon=0;
333
-    rt_mutex_release(&mutex_killReceiveFromMon);
334
-    
335 313
     /**************************************************************************************/
336 314
     /* The task receiveFromMon starts here                                                */
337 315
     /**************************************************************************************/
338 316
     rt_sem_p(&sem_serverOk, TM_INFINITE);
339 317
     cout << "Received message from monitor activated" << endl << flush;
340
-    while (!killReceiveFromMonOk) {
318
+
319
+    while (1) {
341 320
         msgRcv = monitor.Read();
342 321
         cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
343 322
 
344 323
         if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
345 324
             delete(msgRcv);
346
-            cout << "Connection to monitor lost" << endl;
347
-            monitor.Close();
348
-            
349
-            rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
350
-                killReceiveFromMon=1;
351
-            rt_mutex_release(&mutex_killReceiveFromMon);
352
-            
353
-            rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
354
-                killVision=1;
355
-            rt_mutex_release(&mutex_killVision);
356
-            
357
-            rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
358
-                killSendMon=1;
359
-            rt_mutex_release(&mutex_killSendMon);           
360
-             
361
-            rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
362
-                killBattery=1;
363
-            rt_mutex_release(&mutex_killBattery);
364
-
365
-            rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
366
-                killDetectlostSupRob=1;
367
-            rt_mutex_release(&mutex_killDetectLostSupRob);
368
-            
369
-            rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
370
-                acquireImage=0;
371
-            rt_mutex_release(&mutex_acquireImage);            
372
-            
373
-            //exit(-1);
374
-        
375
-        
376
-        } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
377
-            cout << "Command Open Camera Received" << endl << flush;
378
-            //start task Vision
379
-            
325
+            exit(-1);
380 326
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
381
-            cout << "Command Open Communication with Robot Received" << endl << flush;
382 327
             rt_sem_v(&sem_openComRobot);
383
-            
384 328
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
385
-            cout << "Command Start Robot without Watchdog Received" << endl << flush;
386 329
             rt_sem_v(&sem_startRobotWithoutWatchdog);
387
-            
388 330
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
389
-            cout << "Command Start Robot with Watchdog Received" << endl << flush;
390
-            //start task robot with watchdog 
391
-
392
-        } else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
393
-            cout << "Command Search Arena Received" << endl << flush;
394
-            rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
395
-                searchArena=1;
396
-            rt_mutex_release(&mutex_searchArena);
397
-            
398
-        } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
399
-            cout << "Command Get Root Position Received" << endl << flush;
400
-            rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
401
-                getPosition=1;
402
-            rt_mutex_release(&mutex_getPosition);            
403
-
404
-        } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
405
-            cout << "Command Stop Getting Robot Position Received" << endl << flush;
406
-            rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
407
-                getPosition=0;
408
-            rt_mutex_release(&mutex_getPosition); 
409
-
410
-        } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
411
-            cout << "Command Confirm Arena Received" << endl << flush;
412
-            rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
413
-                drawArena=1;
414
-            rt_mutex_release(&mutex_drawArena);
415
-            
416
-            rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
417
-                searchArena=0;
418
-            rt_mutex_release(&mutex_searchArena);
419
-           
420
-            rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
421
-                acquireImage=1;
422
-            rt_mutex_release(&mutex_acquireImage);            
423
- 
424
-        } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
425
-            cout << "Command Infirm Arena Received" << endl << flush;
426
-            rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
427
-                searchArena=0;
428
-            rt_mutex_release(&mutex_searchArena);
429
-            
430
-            rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
431
-                acquireImage=1;
432
-            rt_mutex_release(&mutex_acquireImage);
433
-            
331
+            rt_sem_v(&sem_startRobotWithWatchdog);
434 332
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
435 333
                 msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
436 334
                 msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@@ -441,18 +339,13 @@ void Tasks::ReceiveFromMonTask(void *arg) {
441 339
             move = msgRcv->GetID();
442 340
             rt_mutex_release(&mutex_move);
443 341
         }
444
-        delete(msgRcv); // must be deleted manually, no consumer
445
-        
446
-        //Update loop condition
447
-        rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
448
-        killReceiveFromMonOk=killReceiveFromMon;
449
-        rt_mutex_release(&mutex_killReceiveFromMon);
450
-        
342
+        delete(msgRcv); // mus be deleted manually, no consumer
451 343
     }
452
-    
453
-    rt_sem_v(&sem_allowStartReceive);
454 344
 }
455 345
 
346
+
347
+
348
+
456 349
 /**
457 350
  * @brief Thread opening communication with the robot.
458 351
  */
@@ -486,29 +379,29 @@ void Tasks::OpenComRobot(void *arg) {
486 379
     }
487 380
 }
488 381
 
382
+
383
+
384
+
385
+
386
+
489 387
 /**
490 388
  * @brief Thread starting the communication with the robot.
491
- *//**
492
- * @brief Thread starting the communication with the robot.
493 389
  */
494
-
495 390
 void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
496 391
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
497 392
     // Synchronization barrier (waiting that all tasks are starting)
498 393
     rt_sem_p(&sem_barrier, TM_INFINITE);
499
-    int killBattery=0;
394
+    int killBatteryOk=0;
395
+    Message * msgSend;
500 396
     /**************************************************************************************/
501 397
     /* The task startRobot starts here                                                    */
502 398
     /**************************************************************************************/
503
-    Message* p_mess_answer_battery;
504
-    Message * msgSend;
505
-    rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE);
506
-    cout << "Start robot without watchdog (";
507
-    
508 399
     //Boolean to get the battery
509 400
     rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
510
-    killBattery=0;
401
+    killBatteryOk=0;
511 402
     rt_mutex_release(&mutex_killBattery);
403
+    rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE);
404
+    cout << "Start robot without watchdog (";
512 405
     rt_mutex_acquire(&mutex_robot, TM_INFINITE);
513 406
     msgSend = robot.Write(robot.StartWithoutWD());
514 407
     rt_mutex_release(&mutex_robot);
@@ -522,86 +415,68 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
522 415
         robotStarted = 1;
523 416
         rt_mutex_release(&mutex_robotStarted);
524 417
         rt_task_set_periodic(NULL, TM_NOW, 500000000);
525
-        while (killBattery==0) {
418
+        while (killBatteryOk==0) {
526 419
             rt_task_wait_period(NULL);
527
-            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
528
-            p_mess_answer_battery = robot.Write(robot.GetBattery());
529
-            rt_mutex_release(&mutex_robot);
530
-            rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
531
-            monitor.Write(p_mess_answer_battery);
532
-            rt_mutex_release(&mutex_monitor);
533
-            //cout << endl << flush;
420
+            rt_sem_v(&sem_askBattery);
534 421
             rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
535
-            killBattery=killBattery;
422
+            killBatteryOk=killBattery;
536 423
             rt_mutex_release(&mutex_killBattery);
537 424
         }
538 425
     }
539 426
 }
540 427
 
541
-
542 428
 /**
543 429
  * @brief Thread starting the communication with the robot.
544 430
  *//**
545 431
  * @brief Thread starting the communication with the robot.
546 432
  */
547 433
 
434
+
548 435
 void Tasks::StartRobotTaskWithWatchdog(void *arg) {
549 436
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
550 437
     // Synchronization barrier (waiting that all tasks are starting)
551 438
     rt_sem_p(&sem_barrier, TM_INFINITE);
552
-   
439
+    int killBatteryOk=0;
440
+    int cpt=1;
441
+    Message * msgSend;
553 442
     /**************************************************************************************/
554 443
     /* The task startRobot starts here                                                    */
555 444
     /**************************************************************************************/
556
-    Message* p_mess_answer_battery;
557
-    Message * msgSend;
558
-    int cpt=1;
559
-    int err;
445
+    //Boolean to get the battery
446
+    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
447
+    killBatteryOk=0;
448
+    rt_mutex_release(&mutex_killBattery);
560 449
     rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
561 450
     cout << "Start robot with watchdog (";
562 451
     rt_mutex_acquire(&mutex_robot, TM_INFINITE);
563 452
     msgSend = robot.Write(robot.StartWithWD());
564 453
     rt_mutex_release(&mutex_robot);
565
-    
566
-    //boolean to ask battery
567
-    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
568
-    killBattery=0;
569
-    rt_mutex_release(&mutex_killBattery);
570
-    
571 454
     cout << msgSend->GetID();
572
-    cout << ")" << endl;
573
-        
455
+    cout << ")" << endl;  
574 456
     cout << "Movement answer: " << msgSend->ToString() << endl << flush;
575 457
     WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
576 458
 
577 459
     if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
578
-            
579 460
         rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
580 461
         robotStarted = 1;
581 462
         rt_mutex_release(&mutex_robotStarted);
582 463
         rt_task_set_periodic(NULL, TM_NOW, 500000000);
583
-        while (1) {
464
+        while (killBatteryOk==0) {
584 465
             cpt++;
585
-            if(cpt%2==0){
586
-                rt_mutex_acquire(&mutex_robot, TM_INFINITE);
466
+            if(cpt==2){
587 467
                 robot.Write(robot.ReloadWD());
588
-                rt_mutex_release(&mutex_robot);
468
+                cpt=0;
589 469
             }
590 470
             rt_task_wait_period(NULL);
591
-            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
592
-            p_mess_answer_battery = robot.Write(robot.GetBattery());
593
-            rt_mutex_release(&mutex_robot);
594
-            rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
595
-            monitor.Write(p_mess_answer_battery);
596
-            rt_mutex_release(&mutex_monitor);
597
-            cout << endl << flush;
471
+            rt_sem_v(&sem_askBattery);
472
+            rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
473
+            killBatteryOk=killBattery;
474
+            rt_mutex_release(&mutex_killBattery);
598 475
         }
599 476
     }
600 477
 }
601 478
 
602 479
 
603
-
604
-
605 480
 /**
606 481
  * @brief Thread handling control of the robot.
607 482
  */
@@ -640,6 +515,112 @@ void Tasks::MoveTask(void *arg) {
640 515
 }
641 516
 
642 517
 
518
+
519
+void Tasks::AskBattery(void *arg){
520
+    Message* p_mess_answer_battery;
521
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
522
+    // Synchronization barrier (waiting that all tasks are starting)
523
+    rt_sem_p(&sem_barrier, TM_INFINITE);
524
+    
525
+    /**************************************************************************************/
526
+    /* The task starts here                                                               */
527
+    /**************************************************************************************/
528
+    while(1){
529
+        rt_sem_p(&sem_askBattery, TM_INFINITE);
530
+        rt_mutex_acquire(&mutex_robot, TM_INFINITE);
531
+        p_mess_answer_battery = robot.Write(robot.GetBattery());
532
+        rt_mutex_release(&mutex_robot);
533
+        WriteInQueue(&q_messageToMon, p_mess_answer_battery);
534
+        cout << endl << flush;
535
+    }
536
+}
537
+
538
+
539
+//This task works on a difficult principle that fully make it controllable through 
540
+// monitor, accessing booleans variables to set getPosition or searchArena
541
+
542
+
543
+
544
+void Tasks::Vision(void *arg){
545
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
546
+    // Synchronization barrier (waiting that all tasks are starting)
547
+    rt_sem_p(&sem_barrier, TM_INFINITE);
548
+    
549
+    rt_sem_p(&sem_askBattery, TM_INFINITE);
550
+    int killVisionOk=0;
551
+    Camera camera;
552
+    int acquireImageOk=1;
553
+    int searchArenaOk=0;
554
+    int drawArenaOk=0;
555
+    int getPositionOk=0;
556
+    Arena arena;
557
+    list<Position> positionList;
558
+    list<Position>::iterator it;
559
+    string strPos;
560
+    MessagePosition* msgPos;
561
+    Message* msgPosMsg;
562
+    MessageImg* msgImg;
563
+    Message* msgImgMsg;
564
+    msgPos->SetID(MESSAGE_CAM_POSITION);
565
+    msgImg->SetID(MESSAGE_CAM_IMAGE);
566
+    rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
567
+    killVision=0;
568
+    rt_mutex_release(&mutex_killVision);
569
+    camera.Open();
570
+    while(killVisionOk==0){
571
+        while(acquireImageOk==1){
572
+            Img img=camera.Grab();
573
+            if(searchArenaOk==1){
574
+                rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
575
+                acquireImage=0;
576
+                rt_mutex_release(&mutex_acquireImage);
577
+                acquireImageOk=acquireImage;
578
+                arena=img.SearchArena();
579
+                img.DrawArena(arena);
580
+            }
581
+            if(drawArenaOk==1){
582
+              //  img.DrawArena(arena);
583
+                rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
584
+                acquireImage=1;
585
+                rt_mutex_release(&mutex_acquireImage);
586
+                acquireImageOk=acquireImage;
587
+            }
588
+            if(getPositionOk==1){
589
+                //On démarre la recherche du robot dans la zone définie par l'arène
590
+                positionList=img.SearchRobot(arena);
591
+                //Définitition et assignation de l'itérateur de parcrous de la liste positionList
592
+                it=positionList.begin();
593
+                //Définition d'un messagePosition qui va contenir l'information (x,y)
594
+                msgPos->SetPosition(*it);
595
+                //Transformation en message classique
596
+                msgPosMsg=msgPos->Copy();
597
+                //Envoi
598
+                WriteInQueue(&q_messageToMon,msgPos);
599
+                //Dessis du robot sur l'image
600
+                img.DrawRobot(*it);
601
+            }
602
+            //Définition d'un messageImg contenant l'image avec le robot et l'arène dessinés (ou pas)
603
+            msgImg->SetImage(&img);
604
+            //Transformation en message classique
605
+            msgImgMsg=msgImg->Copy();
606
+            //Envoi
607
+            WriteInQueue(&q_messageToMon,msgImg);
608
+        }
609
+    }
610
+    rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
611
+        searchArena=0;
612
+    rt_mutex_release(&mutex_searchArena);
613
+    searchArenaOk=1;
614
+    rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
615
+        getPosition=0;
616
+    rt_mutex_release(&mutex_getPosition);
617
+    getPositionOk=1;
618
+}
619
+
620
+
621
+
622
+
623
+
643 624
 /**
644 625
  * Write a message in a given queue
645 626
  * @param queue Queue identifier

+ 9
- 1
software/raspberry/superviseur-robot/tasks.h View File

@@ -87,6 +87,8 @@ private:
87 87
     RT_TASK th_startRobotWithoutWatchdog;
88 88
     RT_TASK th_startRobotWithWatchdog;
89 89
     RT_TASK th_move;
90
+    RT_TASK th_askBattery;
91
+    RT_TASK th_vision;
90 92
     
91 93
     /**********************************************************************/
92 94
     /* Mutex                                                              */
@@ -115,7 +117,9 @@ private:
115 117
     RT_SEM sem_serverOk;
116 118
     RT_SEM sem_startRobotWithoutWatchdog;
117 119
     RT_SEM sem_startRobotWithWatchdog;
120
+    RT_SEM sem_askBattery;
118 121
     RT_SEM sem_allowStartReceive;
122
+    RT_SEM sem_allowStartCaptureImage;
119 123
     
120 124
     /**********************************************************************/
121 125
     /* Message queues                                                     */
@@ -161,7 +165,11 @@ private:
161 165
      */
162 166
     void MoveTask(void *arg);
163 167
     
164
-
168
+    
169
+    void AskBattery(void *arg);
170
+    
171
+    
172
+    void Vision(void *arg);
165 173
     
166 174
     /**********************************************************************/
167 175
     /* Queue services                                                     */

Loading…
Cancel
Save