Browse Source

Robot terminé : Tester les fonctionnalités

Leonie Gallois 3 years ago
parent
commit
674485fd95

+ 1
- 1
software/raspberry/superviseur-robot/nbproject/private/configurations.xml View File

@@ -41,7 +41,7 @@
41 41
     </conf>
42 42
     <conf name="Debug__RPI_" type="1">
43 43
       <toolsSet>
44
-        <developmentServer>pi@10.105.1.7:22</developmentServer>
44
+        <developmentServer>pi@10.105.1.07:22</developmentServer>
45 45
         <platform>2</platform>
46 46
       </toolsSet>
47 47
       <dbx_gdbdebugger version="1">

+ 0
- 0
software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.07-pi-22 View File


+ 2
- 0
software/raspberry/superviseur-robot/nbproject/private/private.xml View File

@@ -7,9 +7,11 @@
7 7
     <editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
8 8
     <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
9 9
         <group>
10
+            <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.h</file>
10 11
             <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.cpp</file>
11 12
             <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.cpp</file>
12 13
             <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.h</file>
14
+            <file>file:/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.h</file>
13 15
         </group>
14 16
     </open-files>
15 17
 </project-private>

+ 38
- 0
software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.07-pi-22 View File

@@ -0,0 +1,38 @@
1
+#Wed Mar 17 12:14:13 CET 2021
2
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.h=c1615800255000
3
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/superviseur.doxygen=c1615800255000
4
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/project.xml=c1615800255000
5
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/img.cpp=c1615800255000
6
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/.gitignore=c1615800255000
7
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.h=c1615975893000
8
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/.gitignore=c1615800255000
9
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/commonitor.h=c1615800255000
10
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1615800255000
11
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/.dep.inc=c1615800255000
12
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1615800255000
13
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1615800255000
14
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.h=c1615800255000
15
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1615800255000
16
+VERSION=1.3
17
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1615800255000
18
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/Makefile=c1615800255000
19
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1615800255000
20
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/img.h=c1615800255000
21
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/base64.h=c1615800255000
22
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/tasks.cpp=c1615979649000
23
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1615800255000
24
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/camera.h=c1615800255000
25
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/messages.cpp=c1615800255000
26
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1615800255000
27
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/README.md=c1615800255000
28
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/README.md=c1615800255000
29
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1615800255000
30
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1615800255000
31
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1615800255000
32
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/main.cpp=c1615800255000
33
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1615800255000
34
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/gdbsudo.sh=c1615800255000
35
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/camera.cpp=c1615800255000
36
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1615800255000
37
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1615800255000
38
+/home/gallois/Documents/insa4/S2/robot/Temps_Reel_Robot/software/raspberry/superviseur-robot/nbproject/project.properties=c1615800255000

+ 64
- 23
software/raspberry/superviseur-robot/tasks.cpp View File

@@ -26,7 +26,8 @@
26 26
 #define PRIORITY_TRECEIVEFROMMON 25
27 27
 #define PRIORITY_TSTARTROBOT 20
28 28
 #define PRIORITY_TCAMERA 21
29
-#define PRIORITY_TBATTERY 31
29
+#define PRIORITY_TBATTERY 19
30
+#define PRIORITY_TWATCHDOG 31
30 31
 
31 32
 
32 33
 
@@ -101,7 +102,10 @@ void Tasks::Init() {
101 102
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
102 103
         exit(EXIT_FAILURE);
103 104
     }
104
-    
105
+    if (err = rt_sem_create(&sem_robotStopped, NULL, 0, S_FIFO)) {
106
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
107
+        exit(EXIT_FAILURE);
108
+    }
105 109
     cout << "Semaphores created successfully" << endl << flush;
106 110
 
107 111
     /**************************************************************************************/
@@ -135,6 +139,10 @@ void Tasks::Init() {
135 139
         cerr << "Error task create: " << strerror(-err) << endl << flush;
136 140
         exit(EXIT_FAILURE);
137 141
     }
142
+    if (err = rt_task_create(&th_watchDog, "th_watchDog", 0, PRIORITY_TWATCHDOG, 0)) {
143
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
144
+        exit(EXIT_FAILURE);
145
+    }
138 146
     cout << "Tasks created successfully" << endl << flush;
139 147
 
140 148
     /**************************************************************************************/
@@ -196,7 +204,10 @@ void Tasks::Run() {
196 204
         cerr << "Error task start: " << strerror(-err) << endl << flush;
197 205
         exit(EXIT_FAILURE);
198 206
     }
199
-    
207
+    if (err = rt_task_start(&th_watchDog, (void(*)(void*)) & Tasks::WatchDog, this)) {
208
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
209
+        exit(EXIT_FAILURE);
210
+    }
200 211
     cout << "Tasks launched" << endl << flush;
201 212
 }
202 213
 
@@ -242,7 +253,10 @@ void Tasks::ServerTask(void *arg) {
242 253
         monitor.AcceptClient(); // Wait the monitor client
243 254
         cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
244 255
         rt_sem_broadcast(&sem_serverOk);
245
-        rt_sem_p(&sem_restartServer);
256
+        rt_sem_p(&sem_restartServer, TM_INFINITE);
257
+        rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
258
+        monitor.Close();
259
+        rt_mutex_release(&mutex_monitor);
246 260
     }
247 261
 
248 262
 }
@@ -260,6 +274,7 @@ void Tasks::SendToMonTask(void* arg) {
260 274
     /**************************************************************************************/
261 275
     /* The task sendToMon starts here                                                     */
262 276
     /**************************************************************************************/
277
+    
263 278
     rt_sem_p(&sem_serverOk, TM_INFINITE);
264 279
 
265 280
     while (1) {
@@ -295,10 +310,10 @@ void Tasks::ReceiveFromMonTask(void *arg) {
295 310
 
296 311
             if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
297 312
                 delete(msgRcv);
298
-                WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
299 313
                 WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET));
314
+                WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
300 315
                 WriteInQueue(&q_messageControlCam, new Message(MESSAGE_CAM_CLOSE));
301
-                rt_sem_v(&sem_restartServer, TM_INFINITE);  // A VERIFIER
316
+                rt_sem_v(&sem_restartServer);  // A VERIFIER
302 317
                 break;
303 318
 
304 319
             } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) {
@@ -324,8 +339,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
324 339
                     msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || 
325 340
                     msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) || 
326 341
                     msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) || 
327
-                    msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP) || 
328
-                    )
342
+                    msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP))
329 343
             {
330 344
                 WriteInQueue(&q_messageControlCam, msgRcv);
331 345
             }
@@ -367,7 +381,8 @@ void Tasks::OpenComRobot(void *arg) {
367 381
                 msgSend = new Message(MESSAGE_ANSWER_ACK);
368 382
             }
369 383
             WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
370
-        }else if(msg->CompareID(MESSAGE_ROBOT_COM_CLOSE)){
384
+        }else if(msg->CompareID(MESSAGE_ROBOT_COM_CLOSE)){ 
385
+            rt_sem_p(&sem_robotStopped, TM_INFINITE);
371 386
             cout << "Close serial com (";
372 387
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
373 388
             status = robot.Close();
@@ -383,7 +398,7 @@ void Tasks::OpenComRobot(void *arg) {
383 398
             }
384 399
             WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
385 400
         }
386
-        delete(msg);
401
+        //delete(msg);
387 402
         //rt_sem_p(&sem_openComRobot, TM_INFINITE);
388 403
         
389 404
     }
@@ -423,6 +438,7 @@ void Tasks::StartRobotTask(void *arg) {
423 438
             cout << "Start robot with watchdog (";
424 439
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
425 440
             msgSend = robot.Write(robot.StartWithWD());
441
+            rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
426 442
             rt_mutex_release(&mutex_robot);
427 443
             cout << msgSend->GetID();
428 444
             cout << ")" << endl;
@@ -432,22 +448,29 @@ void Tasks::StartRobotTask(void *arg) {
432 448
                 rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
433 449
                 robotStarted = 1;
434 450
                 rt_mutex_release(&mutex_robotStarted);
435
-                rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
451
+            } else {
452
+                rt_task_set_periodic(&th_watchDog,TM_NOW,0);
436 453
             }
437 454
             
438 455
         } else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){
439 456
             rt_task_set_periodic(&th_watchDog,TM_NOW,0);
440 457
             cout << "Stopping Robot (";
458
+            rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
459
+            robotStarted = 0;
460
+            rt_mutex_release(&mutex_robotStarted);
461
+            rt_task_sleep(100000000);
441 462
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
442
-            msgSend = robot.Write(new Message(MESSAGE_ROBOT_RESET));
463
+            msgSend = robot.Write(robot.Reset());
443 464
             rt_mutex_release(&mutex_robot);
465
+            rt_mutex_acquire(&mutex_move, TM_INFINITE);
466
+            move = MESSAGE_ROBOT_STOP;
467
+            rt_mutex_release(&mutex_move);
444 468
             cout << msgSend->GetID();
445 469
             cout << ")" << endl;
446 470
             cout << "Movement answer: " << msgSend->ToString() << endl << flush;
447 471
             WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
448
-            rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
449
-            robotStarted = 0;
450
-            rt_mutex_release(&mutex_robotStarted);
472
+            
473
+            rt_sem_v(&sem_robotStopped);
451 474
         } 
452 475
         
453 476
    
@@ -461,10 +484,12 @@ void Tasks::WatchDog(void *arg) {
461 484
     rt_sem_p(&sem_barrier, TM_INFINITE);
462 485
     rt_task_set_periodic(NULL, TM_NOW, 0);
463 486
     while (1) {
487
+        cout << "Realoading" << endl << flush;
464 488
         rt_task_wait_period(NULL);
465 489
         Message * msgSend;
466 490
         rt_mutex_acquire(&mutex_robot, TM_INFINITE);
467 491
         msgSend = robot.ReloadWD();
492
+        robot.Write(msgSend);
468 493
         rt_mutex_release(&mutex_robot);
469 494
     }
470 495
 }
@@ -476,6 +501,7 @@ void Tasks::MoveTask(void *arg) {
476 501
     int rs;
477 502
     int previousMove = MESSAGE_ROBOT_GO_FORWARD;
478 503
     int cpMove;
504
+    int nb_error = 0;
479 505
     
480 506
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
481 507
     // Synchronization barrier (waiting that all tasks are starting)
@@ -486,26 +512,43 @@ void Tasks::MoveTask(void *arg) {
486 512
     /**************************************************************************************/
487 513
     rt_task_set_periodic(NULL, TM_NOW, 100000000);
488 514
 
515
+    Message * msgSend;
489 516
     while (1) {
490 517
         rt_task_wait_period(NULL);
491
-        cout << "Periodic movement update";
492 518
         rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
493 519
         rs = robotStarted;
494 520
         rt_mutex_release(&mutex_robotStarted);
495 521
         if (rs == 1) {
522
+            cout << "Periodic movement update";
496 523
             rt_mutex_acquire(&mutex_move, TM_INFINITE);
497 524
             cpMove = move;
498 525
             rt_mutex_release(&mutex_move);
499
-            if (cpMove != previousMove) {
526
+            if (cpMove != previousMove || cpMove != MESSAGE_ROBOT_STOP) {
500 527
                 cout << " move: " << cpMove;
501 528
 
502 529
                 rt_mutex_acquire(&mutex_robot, TM_INFINITE);
503
-                robot.Write(new Message((MessageID)cpMove));
530
+                msgSend = robot.Write(new Message((MessageID)cpMove));
504 531
                 rt_mutex_release(&mutex_robot);
505
-                previousMove = cpMove;
532
+                if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) {
533
+                    nb_error = 0;
534
+                    previousMove = cpMove;
535
+                } else {
536
+                    nb_error++;
537
+                }
538
+                if (nb_error > 2) {
539
+                    cout << "ERREUR ROBOT DETECTED";
540
+                    WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR)); 
541
+                    rt_mutex_acquire(&mutex_move, TM_INFINITE);
542
+                    move = MESSAGE_ROBOT_STOP;
543
+                    rt_mutex_release(&mutex_move);
544
+                    rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
545
+                    robotStarted = 0;
546
+                    rt_mutex_release(&mutex_robotStarted);
547
+                    rt_task_set_periodic(&th_watchDog,TM_NOW,0);
548
+                }
506 549
             }
550
+            cout << endl << flush;
507 551
         }
508
-        cout << endl << flush;
509 552
     }
510 553
 }
511 554
 
@@ -555,11 +598,11 @@ void Tasks::ReadBattery(void *arg){
555 598
 
556 599
     while (1) {
557 600
         rt_task_wait_period(NULL);
558
-        cout << "Periodic battery get lvl \n";
559 601
         rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
560 602
         rs = robotStarted;
561 603
         rt_mutex_release(&mutex_robotStarted);
562 604
         if (rs == 1) {
605
+            cout << "Periodic battery get lvl" << endl << flush;
563 606
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
564 607
             Message *msg = robot.Write( robot.GetBattery() ) ; 
565 608
             rt_mutex_release(&mutex_robot);
@@ -569,7 +612,5 @@ void Tasks::ReadBattery(void *arg){
569 612
                 rt_mutex_release(&mutex_monitor);
570 613
             }
571 614
         }
572
-       
573
-        cout << endl << flush;
574 615
     }
575 616
 }

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

@@ -101,6 +101,7 @@ private:
101 101
     RT_SEM sem_startRobot;
102 102
     RT_SEM sem_watchdogStart;
103 103
     RT_SEM sem_restartServer;
104
+    RT_SEM sem_robotStopped;
104 105
     
105 106
     
106 107
 
@@ -174,7 +175,7 @@ private:
174 175
      * Sends periodically a message to the robot
175 176
      * @return Nothing
176 177
      */
177
-    void Watchdog(void *arg);
178
+    void WatchDog(void *arg);
178 179
 
179 180
 };
180 181
 

Loading…
Cancel
Save