Browse Source

ajout des fichiers tasks.*

Romain Vitrat 4 years ago
parent
commit
181371a74c

+ 655
- 0
software/raspberry/superviseur-robot/tasks.cpp View File

@@ -0,0 +1,655 @@
1
+/*
2
+ * Copyright (C) 2018 dimercur
3
+ *
4
+ * This program is free software: you can redistribute it and/or modify
5
+ * it under the terms of the GNU General Public License as published by
6
+ * the Free Software Foundation, either version 3 of the License, or
7
+ * (at your option) any later version.
8
+ *
9
+ * This program is distributed in the hope that it will be useful,
10
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
11
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12
+ * GNU General Public License for more details.
13
+ *
14
+ * You should have received a copy of the GNU General Public License
15
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16
+ */
17
+
18
+#include "tasks.h"
19
+#include <stdexcept>
20
+
21
+// Déclaration des priorités des taches
22
+#define PRIORITY_TSERVER 30
23
+#define PRIORITY_TOPENCOMROBOT 20
24
+#define PRIORITY_TMOVE 20
25
+#define PRIORITY_TSENDTOMON 22
26
+#define PRIORITY_TRECEIVEFROMMON 25
27
+#define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 24
28
+#define PRIORITY_TSTARTROBOTWITHWATCHDOG 23
29
+#define PRIORITY_TCAMERA 21 
30
+#define PRIORITY_TVISION 26 
31
+
32
+/*
33
+ * Some remarks:
34
+ * 1- This program is mostly a template. It shows you how to create tasks, semaphore
35
+ *   message queues, mutex ... and how to use them
36
+ * 
37
+ * 2- semDumber is, as name say, useless. Its goal is only to show you how to use semaphore
38
+ * 
39
+ * 3- Data flow is probably not optimal
40
+ * 
41
+ * 4- Take into account that ComRobot::Write will block your task when serial buffer is full,
42
+ *   time for internal buffer to flush
43
+ * 
44
+ * 5- Same behavior existe for ComMonitor::Write !
45
+ * 
46
+ * 6- When you want to write something in terminal, use cout and terminate with endl and flush
47
+ * 
48
+ * 7- Good luck !
49
+ */
50
+
51
+/**
52
+ * @brief Initialisation des structures de l'application (tâches, mutex, 
53
+ * semaphore, etc.)
54
+ */
55
+void Tasks::Init() {
56
+    int status;
57
+    int err;
58
+
59
+    /**************************************************************************************/
60
+    /* 	Mutex creation                                                                    */
61
+    /**************************************************************************************/
62
+    if (err = rt_mutex_create(&mutex_monitor, NULL)) {
63
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
64
+        exit(EXIT_FAILURE);
65
+    }
66
+    if (err = rt_mutex_create(&mutex_robot, NULL)) {
67
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
68
+        exit(EXIT_FAILURE);
69
+    }
70
+    if (err = rt_mutex_create(&mutex_robotStarted, NULL)) {
71
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
72
+        exit(EXIT_FAILURE);
73
+    }
74
+    if (err = rt_mutex_create(&mutex_move, NULL)) {
75
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
76
+        exit(EXIT_FAILURE);
77
+    }
78
+    if (err = rt_mutex_create(&mutex_killBattery, NULL)) {
79
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
80
+        exit(EXIT_FAILURE);
81
+    }
82
+    if (err = rt_mutex_create(&mutex_killVision, NULL)) {
83
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
84
+        exit(EXIT_FAILURE);
85
+    }
86
+    if (err = rt_mutex_create(&mutex_acquireImage, NULL)) {
87
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
88
+        exit(EXIT_FAILURE);
89
+    }
90
+    if (err = rt_mutex_create(&mutex_searchArena, NULL)) {
91
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
92
+        exit(EXIT_FAILURE);
93
+    }
94
+    if (err = rt_mutex_create(&mutex_drawArena, NULL)) {
95
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
96
+        exit(EXIT_FAILURE);
97
+    }
98
+    if (err = rt_mutex_create(&mutex_getPosition, NULL)) {
99
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
100
+        exit(EXIT_FAILURE);
101
+    }
102
+    cout << "Mutexes created successfully" << endl << flush;
103
+
104
+    /**************************************************************************************/
105
+    /* 	Semaphors creation       							  */
106
+    /**************************************************************************************/
107
+    if (err = rt_sem_create(&sem_barrier, NULL, 0, S_FIFO)) {
108
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
109
+        exit(EXIT_FAILURE);
110
+    }
111
+    if (err = rt_sem_create(&sem_openComRobot, NULL, 0, S_FIFO)) {
112
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
113
+        exit(EXIT_FAILURE);
114
+    }
115
+    if (err = rt_sem_create(&sem_serverOk, NULL, 0, S_FIFO)) {
116
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
117
+        exit(EXIT_FAILURE);
118
+    }
119
+    if (err = rt_sem_create(&sem_startRobotWithoutWatchdog, NULL, 0, S_FIFO)) {
120
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
121
+        exit(EXIT_FAILURE);
122
+    }
123
+    if (err = rt_sem_create(&sem_startRobotWithWatchdog, NULL, 0, S_FIFO)) {
124
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
125
+        exit(EXIT_FAILURE);
126
+    }
127
+    if (err = rt_sem_create(&sem_askBattery, NULL, 0, S_FIFO)) {
128
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
129
+        exit(EXIT_FAILURE);
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
+    }
135
+    cout << "Semaphores created successfully" << endl << flush;
136
+
137
+    /**************************************************************************************/
138
+    /* Tasks creation                                                                     */
139
+    /**************************************************************************************/
140
+    if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) {
141
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
142
+        exit(EXIT_FAILURE);
143
+    }
144
+    if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) {
145
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
146
+        exit(EXIT_FAILURE);
147
+    }
148
+    if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) {
149
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
150
+        exit(EXIT_FAILURE);
151
+    }
152
+    if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) {
153
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
154
+        exit(EXIT_FAILURE);
155
+    }
156
+    if (err = rt_task_create(&th_startRobotWithoutWatchdog, "th_startRobotWithoutWatchdog", 0, PRIORITY_TSTARTROBOTWITHOUTWATCHDOG, 0)) {
157
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
158
+        exit(EXIT_FAILURE);
159
+    }
160
+    if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOTWITHWATCHDOG, 0)) {
161
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
162
+        exit(EXIT_FAILURE);
163
+    }
164
+    if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
165
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
166
+        exit(EXIT_FAILURE);
167
+    }
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
+    }
176
+    cout << "Tasks created successfully" << endl << flush;
177
+
178
+    /**************************************************************************************/
179
+    /* Message queues creation                                                            */
180
+    /**************************************************************************************/
181
+    if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
182
+        cerr << "Error msg queue create: " << strerror(-err) << endl << flush;
183
+        exit(EXIT_FAILURE);
184
+    }
185
+    cout << "Queues created successfully" << endl << flush;
186
+
187
+}
188
+
189
+/**
190
+ * @brief Démarrage des tâches
191
+ */
192
+void Tasks::Run() {
193
+    rt_task_set_priority(NULL, T_LOPRIO);
194
+    int err;
195
+
196
+    if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
197
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
198
+        exit(EXIT_FAILURE);
199
+    }
200
+    if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
201
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
202
+        exit(EXIT_FAILURE);
203
+    }
204
+    if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) {
205
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
206
+        exit(EXIT_FAILURE);
207
+    }
208
+    if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
209
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
210
+        exit(EXIT_FAILURE);
211
+    }
212
+    if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) {
213
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
214
+        exit(EXIT_FAILURE);
215
+    }
216
+    if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) {
217
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
218
+        exit(EXIT_FAILURE);
219
+    }
220
+    if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) {
221
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
222
+        exit(EXIT_FAILURE);
223
+    }
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
+    }*/
232
+    cout << "Tasks launched" << endl << flush;
233
+}
234
+
235
+/**
236
+ * @brief Arrêt des tâches
237
+ */
238
+void Tasks::Stop() {
239
+    monitor.Close();
240
+    robot.Close();
241
+}
242
+
243
+/**
244
+ */
245
+void Tasks::Join() {
246
+    cout << "Tasks synchronized" << endl << flush;
247
+    rt_sem_broadcast(&sem_barrier);
248
+    pause();
249
+}
250
+
251
+/**
252
+ * @brief Thread handling server communication with the monitor.
253
+ */
254
+void Tasks::ServerTask(void *arg) {
255
+    int status;
256
+    
257
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
258
+    // Synchronization barrier (waiting that all tasks are started)
259
+    rt_sem_p(&sem_barrier, TM_INFINITE);
260
+
261
+    /**************************************************************************************/
262
+    /* The task server starts here                                                        */
263
+    /**************************************************************************************/
264
+    rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
265
+    status = monitor.Open(SERVER_PORT);
266
+    rt_mutex_release(&mutex_monitor);
267
+
268
+    cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
269
+
270
+    if (status < 0) throw std::runtime_error {
271
+        "Unable to start server on port " + std::to_string(SERVER_PORT)
272
+    };
273
+    monitor.AcceptClient(); // Wait the monitor client
274
+    cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
275
+    rt_sem_broadcast(&sem_serverOk);
276
+}
277
+
278
+/**
279
+ * @brief Thread sending data to monitor.
280
+ */
281
+void Tasks::SendToMonTask(void* arg) {
282
+    Message *msg;
283
+    
284
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
285
+    // Synchronization barrier (waiting that all tasks are starting)
286
+    rt_sem_p(&sem_barrier, TM_INFINITE);
287
+
288
+    /**************************************************************************************/
289
+    /* The task sendToMon starts here                                                     */
290
+    /**************************************************************************************/
291
+    rt_sem_p(&sem_serverOk, TM_INFINITE);
292
+
293
+    while (1) {
294
+        cout << "wait msg to send" << endl << flush;
295
+        msg = ReadInQueue(&q_messageToMon);
296
+        cout << "Send msg to mon: " << msg->ToString() << endl << flush;
297
+        rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
298
+        monitor.Write(msg); // The message is deleted with the Write
299
+        rt_mutex_release(&mutex_monitor);
300
+    }
301
+}
302
+
303
+/**
304
+ * @brief Thread receiving data from monitor.
305
+ */
306
+void Tasks::ReceiveFromMonTask(void *arg) {
307
+    Message *msgRcv;
308
+    
309
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
310
+    // Synchronization barrier (waiting that all tasks are starting)
311
+    rt_sem_p(&sem_barrier, TM_INFINITE);
312
+    
313
+    /**************************************************************************************/
314
+    /* The task receiveFromMon starts here                                                */
315
+    /**************************************************************************************/
316
+    rt_sem_p(&sem_serverOk, TM_INFINITE);
317
+    cout << "Received message from monitor activated" << endl << flush;
318
+
319
+    while (1) {
320
+        msgRcv = monitor.Read();
321
+        cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
322
+
323
+        if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
324
+            delete(msgRcv);
325
+            exit(-1);
326
+        } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
327
+            rt_sem_v(&sem_openComRobot);
328
+        } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
329
+            rt_sem_v(&sem_startRobotWithoutWatchdog);
330
+        } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
331
+            rt_sem_v(&sem_startRobotWithWatchdog);
332
+        } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
333
+                msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
334
+                msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
335
+                msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
336
+                msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
337
+
338
+            rt_mutex_acquire(&mutex_move, TM_INFINITE);
339
+            move = msgRcv->GetID();
340
+            rt_mutex_release(&mutex_move);
341
+        }
342
+        delete(msgRcv); // mus be deleted manually, no consumer
343
+    }
344
+}
345
+
346
+
347
+
348
+
349
+/**
350
+ * @brief Thread opening communication with the robot.
351
+ */
352
+void Tasks::OpenComRobot(void *arg) {
353
+    int status;
354
+    int err;
355
+
356
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
357
+    // Synchronization barrier (waiting that all tasks are starting)
358
+    rt_sem_p(&sem_barrier, TM_INFINITE);
359
+    
360
+    /**************************************************************************************/
361
+    /* The task openComRobot starts here                                                  */
362
+    /**************************************************************************************/
363
+    while (1) {
364
+        rt_sem_p(&sem_openComRobot, TM_INFINITE);
365
+        cout << "Open serial com (";
366
+        rt_mutex_acquire(&mutex_robot, TM_INFINITE);
367
+        status = robot.Open();
368
+        rt_mutex_release(&mutex_robot);
369
+        cout << status;
370
+        cout << ")" << endl << flush;
371
+
372
+        Message * msgSend;
373
+        if (status < 0) {
374
+            msgSend = new Message(MESSAGE_ANSWER_NACK);
375
+        } else {
376
+            msgSend = new Message(MESSAGE_ANSWER_ACK);
377
+        }
378
+        WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
379
+    }
380
+}
381
+
382
+
383
+
384
+
385
+
386
+
387
+/**
388
+ * @brief Thread starting the communication with the robot.
389
+ */
390
+void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
391
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
392
+    // Synchronization barrier (waiting that all tasks are starting)
393
+    rt_sem_p(&sem_barrier, TM_INFINITE);
394
+    int killBatteryOk=0;
395
+    Message * msgSend;
396
+    /**************************************************************************************/
397
+    /* The task startRobot starts here                                                    */
398
+    /**************************************************************************************/
399
+    //Boolean to get the battery
400
+    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
401
+    killBatteryOk=0;
402
+    rt_mutex_release(&mutex_killBattery);
403
+    rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE);
404
+    cout << "Start robot without watchdog (";
405
+    rt_mutex_acquire(&mutex_robot, TM_INFINITE);
406
+    msgSend = robot.Write(robot.StartWithoutWD());
407
+    rt_mutex_release(&mutex_robot);
408
+    cout << msgSend->GetID();
409
+    cout << ")" << endl;  
410
+    cout << "Movement answer: " << msgSend->ToString() << endl << flush;
411
+    WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
412
+
413
+    if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
414
+        rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
415
+        robotStarted = 1;
416
+        rt_mutex_release(&mutex_robotStarted);
417
+        rt_task_set_periodic(NULL, TM_NOW, 500000000);
418
+        while (killBatteryOk==0) {
419
+            rt_task_wait_period(NULL);
420
+            rt_sem_v(&sem_askBattery);
421
+            rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
422
+            killBatteryOk=killBattery;
423
+            rt_mutex_release(&mutex_killBattery);
424
+        }
425
+    }
426
+}
427
+
428
+/**
429
+ * @brief Thread starting the communication with the robot.
430
+ *//**
431
+ * @brief Thread starting the communication with the robot.
432
+ */
433
+
434
+
435
+void Tasks::StartRobotTaskWithWatchdog(void *arg) {
436
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
437
+    // Synchronization barrier (waiting that all tasks are starting)
438
+    rt_sem_p(&sem_barrier, TM_INFINITE);
439
+    int killBatteryOk=0;
440
+    int cpt=1;
441
+    Message * msgSend;
442
+    /**************************************************************************************/
443
+    /* The task startRobot starts here                                                    */
444
+    /**************************************************************************************/
445
+    //Boolean to get the battery
446
+    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
447
+    killBattery=0;
448
+    rt_mutex_release(&mutex_killBattery);
449
+    rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
450
+    cout << "Start robot with watchdog (";
451
+    rt_mutex_acquire(&mutex_robot, TM_INFINITE);
452
+    msgSend = robot.Write(robot.StartWithWD());
453
+    rt_mutex_release(&mutex_robot);
454
+    cout << msgSend->GetID();
455
+    cout << ")" << endl;  
456
+    cout << "Movement answer: " << msgSend->ToString() << endl << flush;
457
+    WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon
458
+
459
+    if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
460
+        rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
461
+        robotStarted = 1;
462
+        rt_mutex_release(&mutex_robotStarted);
463
+        rt_task_set_periodic(NULL, TM_NOW, 500000000);
464
+        while (killBatteryOk==0) {
465
+            cpt++;
466
+            if(cpt==2){
467
+                robot.Write(robot.ReloadWD());
468
+                cpt=0;
469
+            }
470
+            rt_task_wait_period(NULL);
471
+            rt_sem_v(&sem_askBattery);
472
+            rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
473
+            killBatteryOk=killBattery;
474
+            rt_mutex_release(&mutex_killBattery);
475
+        }
476
+    }
477
+}
478
+
479
+
480
+/**
481
+ * @brief Thread handling control of the robot.
482
+ */
483
+void Tasks::MoveTask(void *arg) {
484
+    int rs;
485
+    int cpMove;
486
+    
487
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
488
+    // Synchronization barrier (waiting that all tasks are starting)
489
+    rt_sem_p(&sem_barrier, TM_INFINITE);
490
+    
491
+    /**************************************************************************************/
492
+    /* The task starts here                                                               */
493
+    /**************************************************************************************/
494
+    rt_task_set_periodic(NULL, TM_NOW, 100000000);
495
+
496
+    while (1) {
497
+        rt_task_wait_period(NULL);
498
+        cout << "Periodic movement update";
499
+        rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
500
+        rs = robotStarted;
501
+        rt_mutex_release(&mutex_robotStarted);
502
+        if (rs == 1) {
503
+            rt_mutex_acquire(&mutex_move, TM_INFINITE);
504
+            cpMove = move;
505
+            rt_mutex_release(&mutex_move);
506
+            
507
+            cout << " move: " << cpMove;
508
+            
509
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
510
+            robot.Write(new Message((MessageID)cpMove));
511
+            rt_mutex_release(&mutex_robot);
512
+        }
513
+        cout << endl << flush;
514
+    }
515
+}
516
+
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
+
624
+/**
625
+ * Write a message in a given queue
626
+ * @param queue Queue identifier
627
+ * @param msg Message to be stored
628
+ */
629
+void Tasks::WriteInQueue(RT_QUEUE *queue, Message *msg) {
630
+    int err;
631
+    if ((err = rt_queue_write(queue, (const void *) &msg, sizeof ((const void *) &msg), Q_NORMAL)) < 0) {
632
+        cerr << "Write in queue failed: " << strerror(-err) << endl << flush;
633
+        throw std::runtime_error{"Error in write in queue"};
634
+    }
635
+}
636
+
637
+/**
638
+ * Read a message from a given queue, block if empty
639
+ * @param queue Queue identifier
640
+ * @return Message read
641
+ */
642
+Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
643
+    int err;
644
+    Message *msg;
645
+
646
+    if ((err = rt_queue_read(queue, &msg, sizeof ((void*) &msg), TM_INFINITE)) < 0) {
647
+        cout << "Read in queue failed: " << strerror(-err) << endl << flush;
648
+        throw std::runtime_error{"Error in read in queue"};
649
+    }/** else {
650
+        cout << "@msg :" << msg << endl << flush;
651
+    } /**/
652
+
653
+    return msg;
654
+}
655
+

+ 188
- 0
software/raspberry/superviseur-robot/tasks.h View File

@@ -0,0 +1,188 @@
1
+/*
2
+ * Copyright (C) 2018 dimercur
3
+ *
4
+ * This program is free software: you can redistribute it and/or modify
5
+ * it under the terms of the GNU General Public License as published by
6
+ * the Free Software Foundation, either version 3 of the License, or
7
+ * (at your option) any later version.
8
+ *
9
+ * This program is distributed in the hope that it will be useful,
10
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
11
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12
+ * GNU General Public License for more details.
13
+ *
14
+ * You should have received a copy of the GNU General Public License
15
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16
+ */
17
+
18
+#ifndef __TASKS_H__
19
+#define __TASKS_H__
20
+
21
+#include <unistd.h>
22
+#include <iostream>
23
+
24
+#include <sys/mman.h>
25
+#include <alchemy/task.h>
26
+#include <alchemy/timer.h>
27
+#include <alchemy/mutex.h>
28
+#include <alchemy/sem.h>
29
+#include <alchemy/queue.h>
30
+
31
+#include "messages.h"
32
+#include "commonitor.h"
33
+#include "comrobot.h"
34
+#include "camera.h"
35
+#include "img.h"
36
+
37
+using namespace std;
38
+
39
+class Tasks {
40
+public:
41
+    /**
42
+     * @brief Initializes main structures (semaphores, tasks, mutex, etc.)
43
+     */
44
+    void Init();
45
+
46
+    /**
47
+     * @brief Starts tasks
48
+     */
49
+    void Run();
50
+
51
+    /**
52
+     * @brief Stops tasks
53
+     */
54
+    void Stop();
55
+    
56
+    /**
57
+     * @brief Suspends main thread
58
+     */
59
+    void Join();
60
+    
61
+private:
62
+    /**********************************************************************/
63
+    /* Shared data                                                        */
64
+    /**********************************************************************/
65
+    ComMonitor monitor;
66
+    ComRobot robot;
67
+    int robotStarted = 0;
68
+    int move = MESSAGE_ROBOT_STOP;
69
+    bool killBattery = 0;
70
+    bool killReceiveFromMon=0;
71
+    bool killVision=0;
72
+    bool searchArena=0;
73
+    bool killSendMon=0;
74
+    bool killDetectlostSupRob=0;
75
+    bool acquireImage=0;
76
+    bool getPosition=0;
77
+    bool drawArena=0;
78
+    
79
+    /**********************************************************************/
80
+    /* Tasks                                                              */
81
+    /**********************************************************************/
82
+    RT_TASK th_server;
83
+    RT_TASK th_sendToMon;
84
+    RT_TASK th_receiveFromMon;
85
+    RT_TASK th_openComRobot;
86
+    RT_TASK th_startRobotWithoutWatchdog;
87
+    RT_TASK th_startRobotWithWatchdog;
88
+    RT_TASK th_move;
89
+    RT_TASK th_vision;
90
+    RT_TASK th_askBattery;
91
+    
92
+    /**********************************************************************/
93
+    /* Mutex                                                              */
94
+    /**********************************************************************/
95
+    RT_MUTEX mutex_monitor;
96
+    RT_MUTEX mutex_robot;
97
+    RT_MUTEX mutex_robotStarted;
98
+    RT_MUTEX mutex_move;
99
+    RT_MUTEX mutex_killBattery;
100
+    RT_MUTEX mutex_killVision;
101
+    RT_MUTEX mutex_acquireImage;
102
+    RT_MUTEX mutex_searchArena;
103
+    RT_MUTEX mutex_drawArena;
104
+    RT_MUTEX mutex_getPosition;
105
+
106
+    /**********************************************************************/
107
+    /* Semaphores                                                         */
108
+    /**********************************************************************/
109
+    RT_SEM sem_barrier;
110
+    RT_SEM sem_openComRobot;
111
+    RT_SEM sem_serverOk;
112
+    RT_SEM sem_startRobotWithoutWatchdog;
113
+    RT_SEM sem_startRobotWithWatchdog;
114
+    RT_SEM sem_askBattery;
115
+    RT_SEM sem_allowStartCaptureImage;
116
+    
117
+    /**********************************************************************/
118
+    /* Message queues                                                     */
119
+    /**********************************************************************/
120
+    int MSG_QUEUE_SIZE;
121
+    RT_QUEUE q_messageToMon;
122
+    
123
+    /**********************************************************************/
124
+    /* Tasks' functions                                                   */
125
+    /**********************************************************************/
126
+    /**
127
+     * @brief Thread handling server communication with the monitor.
128
+     */
129
+    void ServerTask(void *arg);
130
+     
131
+    /**
132
+     * @brief Thread sending data to monitor.
133
+     */
134
+    void SendToMonTask(void *arg);
135
+        
136
+    /**
137
+     * @brief Thread receiving data from monitor.
138
+     */
139
+    void ReceiveFromMonTask(void *arg);
140
+    
141
+    /**
142
+     * @brief Thread opening communication with the robot.
143
+     */
144
+    void OpenComRobot(void *arg);
145
+
146
+    /**
147
+     * @brief Thread starting the communication with the robot.
148
+     */
149
+    void StartRobotTaskWithoutWatchdog(void *arg);
150
+    
151
+    /**
152
+     * @brief Thread starting the communication with the robot with watchdog.
153
+     */
154
+    void StartRobotTaskWithWatchdog(void *arg);
155
+    
156
+    
157
+    /**
158
+     * @brief Thread handling control of the robot.
159
+     */
160
+    void MoveTask(void *arg);
161
+    
162
+    
163
+    void AskBattery(void *arg);
164
+    
165
+    
166
+    void Vision(void *arg);
167
+    
168
+    /**********************************************************************/
169
+    /* Queue services                                                     */
170
+    /**********************************************************************/
171
+    /**
172
+     * Write a message in a given queue
173
+     * @param queue Queue identifier
174
+     * @param msg Message to be stored
175
+     */
176
+    void WriteInQueue(RT_QUEUE *queue, Message *msg);
177
+    
178
+    /**
179
+     * Read a message from a given queue, block if empty
180
+     * @param queue Queue identifier
181
+     * @return Message read
182
+     */
183
+    Message *ReadInQueue(RT_QUEUE *queue);
184
+
185
+};
186
+
187
+#endif // __TASKS_H__ 
188
+

Loading…
Cancel
Save