No Description
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

tasks.cpp 31KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784
  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. #include "tasks.h"
  18. #include <stdexcept>
  19. // Déclaration des priorités des taches
  20. #define PRIORITY_TSERVER 30
  21. #define PRIORITY_TOPENCOMROBOT 20
  22. #define PRIORITY_TMOVE 20
  23. #define PRIORITY_TSENDTOMON 22
  24. #define PRIORITY_TRECEIVEFROMMON 25
  25. #define PRIORITY_TSTARTROBOT 20
  26. #define PRIORITY_TCAMERA 21
  27. #define PRIORITY_TWATCHDOG 35
  28. #define PRIORITY_TBATTERY 20
  29. /*
  30. * Some remarks:
  31. * 1- This program is mostly a template. It shows you how to create tasks, semaphore
  32. * message queues, mutex ... and how to use them
  33. *
  34. * 2- semDumber is, as name say, useless. Its goal is only to show you how to use semaphore
  35. *
  36. * 3- Data flow is probably not optimal
  37. *
  38. * 4- Take into account that ComRobot::Write will block your task when serial buffer is full,
  39. * time for internal buffer to flush
  40. *
  41. * 5- Same behavior existe for ComMonitor::Write !
  42. *
  43. * 6- When you want to write something in terminal, use cout and terminate with endl and flush
  44. *
  45. * 7- Good luck !
  46. */
  47. /**
  48. * @brief Initialisation des structures de l'application (tâches, mutex,
  49. * semaphore, etc.)
  50. */
  51. void Tasks::Init() {
  52. int status;
  53. int err;
  54. /**************************************************************************************/
  55. /* Mutex creation */
  56. /**************************************************************************************/
  57. if (err = rt_mutex_create(&mutex_monitor, NULL)) {
  58. cerr << "Error mutex create: " << strerror(-err) << endl << flush;
  59. exit(EXIT_FAILURE);
  60. }
  61. if (err = rt_mutex_create(&mutex_robot, NULL)) {
  62. cerr << "Error mutex create: " << strerror(-err) << endl << flush;
  63. exit(EXIT_FAILURE);
  64. }
  65. if (err = rt_mutex_create(&mutex_robotStarted, NULL)) {
  66. cerr << "Error mutex create: " << strerror(-err) << endl << flush;
  67. exit(EXIT_FAILURE);
  68. }
  69. if (err = rt_mutex_create(&mutex_move, NULL)) {
  70. cerr << "Error mutex create: " << strerror(-err) << endl << flush;
  71. exit(EXIT_FAILURE);
  72. }
  73. if (err = rt_mutex_create(&mutex_watchdog, NULL)) {
  74. cerr << "Error mutex create: " << strerror(-err) << endl << flush;
  75. exit(EXIT_FAILURE);
  76. }
  77. if (err = rt_mutex_create(&mutex_etatCamera, NULL)) {
  78. cerr << "Error mutex create: " << strerror(-err) << endl << flush;
  79. exit(EXIT_FAILURE);
  80. }
  81. cout << "Mutexes created successfully" << endl << flush;
  82. /**************************************************************************************/
  83. /* Semaphors creation */
  84. /**************************************************************************************/
  85. if (err = rt_sem_create(&sem_barrier, NULL, 0, S_FIFO)) {
  86. cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
  87. exit(EXIT_FAILURE);
  88. }
  89. if (err = rt_sem_create(&sem_openComRobot, NULL, 0, S_FIFO)) {
  90. cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
  91. exit(EXIT_FAILURE);
  92. }
  93. if (err = rt_sem_create(&sem_serverOk, NULL, 0, S_FIFO)) {
  94. cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
  95. exit(EXIT_FAILURE);
  96. }
  97. if (err = rt_sem_create(&sem_startRobot, NULL, 0, S_FIFO)) {
  98. cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
  99. exit(EXIT_FAILURE);
  100. }
  101. if (err = rt_sem_create(&sem_startWD, NULL, 0, S_FIFO)) {
  102. cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
  103. exit(EXIT_FAILURE);
  104. }
  105. if (err = rt_sem_create(&sem_startCAM, NULL, 0, S_FIFO)) {
  106. cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
  107. exit(EXIT_FAILURE);
  108. }
  109. cout << "Semaphores created successfully" << endl << flush;
  110. /**************************************************************************************/
  111. /* Tasks creation */
  112. /**************************************************************************************/
  113. if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) {
  114. cerr << "Error task create: " << strerror(-err) << endl << flush;
  115. exit(EXIT_FAILURE);
  116. }
  117. if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) {
  118. cerr << "Error task create: " << strerror(-err) << endl << flush;
  119. exit(EXIT_FAILURE);
  120. }
  121. if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) {
  122. cerr << "Error task create: " << strerror(-err) << endl << flush;
  123. exit(EXIT_FAILURE);
  124. }
  125. if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) {
  126. cerr << "Error task create: " << strerror(-err) << endl << flush;
  127. exit(EXIT_FAILURE);
  128. }
  129. if (err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0)) {
  130. cerr << "Error task create: " << strerror(-err) << endl << flush;
  131. exit(EXIT_FAILURE);
  132. }
  133. if (err = rt_task_create(&th_battery, "th_battery", 0, PRIORITY_TBATTERY, 0)) {
  134. cerr << "Error task create: " << strerror(-err) << endl << flush;
  135. exit(EXIT_FAILURE);
  136. }
  137. if (err = rt_task_create(&th_robot, "th_robot", 0, PRIORITY_TMOVE, 0)) {
  138. cerr << "Error task create: " << strerror(-err) << endl << flush;
  139. exit(EXIT_FAILURE);
  140. }
  141. if (err = rt_task_create(&th_watchdog, "th_watchdog", 0, PRIORITY_TWATCHDOG, 0)) {
  142. cerr << "Error task create: " << strerror(-err) << endl << flush;
  143. exit(EXIT_FAILURE);
  144. }
  145. if (err = rt_task_create(&th_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) {
  146. cerr << "Error task create: " << strerror(-err) << endl << flush;
  147. exit(EXIT_FAILURE);
  148. }
  149. if (err = rt_task_create(&th_startCamera, "th_startCamera", 0, PRIORITY_TCAMERA, 0)) {
  150. cerr << "Error task create: " << strerror(-err) << endl << flush;
  151. exit(EXIT_FAILURE);
  152. }
  153. cout << "Tasks created successfully" << endl << flush;
  154. /**************************************************************************************/
  155. /* Message queues creation */
  156. /**************************************************************************************/
  157. if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
  158. cerr << "Error msg queue create: " << strerror(-err) << endl << flush;
  159. exit(EXIT_FAILURE);
  160. }
  161. cout << "Queues created successfully" << endl << flush;
  162. }
  163. /**
  164. * @brief Démarrage des tâches
  165. */
  166. void Tasks::Run() {
  167. rt_task_set_priority(NULL, T_LOPRIO);
  168. int err;
  169. if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
  170. cerr << "Error task start: " << strerror(-err) << endl << flush;
  171. exit(EXIT_FAILURE);
  172. }
  173. if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
  174. cerr << "Error task start: " << strerror(-err) << endl << flush;
  175. exit(EXIT_FAILURE);
  176. }
  177. if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) {
  178. cerr << "Error task start: " << strerror(-err) << endl << flush;
  179. exit(EXIT_FAILURE);
  180. }
  181. if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
  182. cerr << "Error task start: " << strerror(-err) << endl << flush;
  183. exit(EXIT_FAILURE);
  184. }
  185. if (err = rt_task_start(&th_startRobot, (void(*)(void*)) & Tasks::StartRobotTask, this)) {
  186. cerr << "Error task start: " << strerror(-err) << endl << flush;
  187. exit(EXIT_FAILURE);
  188. }
  189. if (err = rt_task_start(&th_robot, (void(*)(void*)) & Tasks::MoveTask, this)) {
  190. cerr << "Error task start: " << strerror(-err) << endl << flush;
  191. exit(EXIT_FAILURE);
  192. }
  193. if (err = rt_task_start(&th_battery, (void(*)(void*)) & Tasks::SendBatteryTask, this)) {
  194. cerr << "Error task start: " << strerror(-err) << endl << flush;
  195. exit(EXIT_FAILURE);
  196. }
  197. if (err = rt_task_start(&th_startCamera, (void(*)(void*)) & Tasks::StartCamera, this)) {
  198. cerr << "Error task start: " << strerror(-err) << endl << flush;
  199. exit(EXIT_FAILURE);
  200. }
  201. if (err = rt_task_start(&th_watchdog, (void(*)(void*)) & Tasks::ReloadWatchdogTask, this)) {
  202. cerr << "Error task start: " << strerror(-err) << endl << flush;
  203. exit(EXIT_FAILURE);
  204. }
  205. if (err = rt_task_start(&th_camera, (void(*)(void*)) & Tasks::CameraTask, this)) {
  206. cerr << "Error task start: " << strerror(-err) << endl << flush;
  207. exit(EXIT_FAILURE);
  208. }
  209. cout << "Tasks launched" << endl << flush;
  210. }
  211. /**
  212. * @brief Arrêt des tâches
  213. */
  214. void Tasks::Stop() {
  215. camera.Close();
  216. monitor.Close();
  217. robot.Close();
  218. }
  219. /**
  220. */
  221. void Tasks::Join() {
  222. cout << "Tasks synchronized" << endl << flush;
  223. rt_sem_broadcast(&sem_barrier);
  224. pause();
  225. }
  226. /**
  227. * @brief Thread handling server communication with the monitor.
  228. */
  229. void Tasks::ServerTask(void *arg) {
  230. int status;
  231. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  232. // Synchronization barrier (waiting that all tasks are started)
  233. rt_sem_p(&sem_barrier, TM_INFINITE);
  234. /**************************************************************************************/
  235. /* The task server starts here */
  236. /**************************************************************************************/
  237. rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
  238. status = monitor.Open(SERVER_PORT);
  239. rt_mutex_release(&mutex_monitor);
  240. cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
  241. if (status < 0) throw std::runtime_error {
  242. "Unable to start server on port " + std::to_string(SERVER_PORT)
  243. };
  244. monitor.AcceptClient(); // Wait the monitor client
  245. cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
  246. rt_sem_broadcast(&sem_serverOk);
  247. }
  248. /**
  249. * @brief Thread sending data to monitor.
  250. */
  251. void Tasks::SendToMonTask(void* arg) {
  252. Message *msg;
  253. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  254. // Synchronization barrier (waiting that all tasks are starting)
  255. rt_sem_p(&sem_barrier, TM_INFINITE);
  256. /**************************************************************************************/
  257. /* The task sendToMon starts here */
  258. /**************************************************************************************/
  259. rt_sem_p(&sem_serverOk, TM_INFINITE);
  260. while (1) {
  261. cout << "wait msg to send" << endl << flush;
  262. msg = ReadInQueue(&q_messageToMon);
  263. cout << "Send msg to mon: " << msg->ToString() << endl << flush;
  264. rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
  265. monitor.Write(msg); // The message is deleted with the Write
  266. rt_mutex_release(&mutex_monitor);
  267. }
  268. }
  269. /**
  270. * @brief Thread receiving data from monitor.
  271. */
  272. void Tasks::ReceiveFromMonTask(void *arg) {
  273. Message *msgRcv;
  274. int err;
  275. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  276. // Synchronization barrier (waiting that all tasks are starting)
  277. rt_sem_p(&sem_barrier, TM_INFINITE);
  278. /**************************************************************************************/
  279. /* The task receiveFromMon starts here */
  280. /**************************************************************************************/
  281. while(1){
  282. rt_sem_p(&sem_serverOk, TM_INFINITE);
  283. cout << "Received message from monitor activated" << endl << flush;
  284. while (1) {
  285. msgRcv = monitor.Read();
  286. cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
  287. if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
  288. cout << "On est dans le message monitor lost" << endl << flush;
  289. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  290. int cpMove = MESSAGE_ROBOT_STOP;
  291. robot.Write(new Message((MessageID)cpMove));
  292. rt_mutex_release(&mutex_robot);
  293. robot.Close();
  294. monitor.Close();
  295. rt_mutex_acquire(&mutex_move, TM_INFINITE);
  296. move = MESSAGE_ROBOT_STOP;
  297. rt_mutex_release(&mutex_move);
  298. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  299. robotStarted = 0;
  300. rt_mutex_release(&mutex_robotStarted);
  301. rt_mutex_acquire(&mutex_watchdog, TM_INFINITE);
  302. watchdog = 0;
  303. rt_mutex_release(&mutex_watchdog);
  304. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  305. etatCamera.arena = Arena();
  306. etatCamera.askArena = 0;
  307. etatCamera.cameraStarted = 0;
  308. etatCamera.getImg = 0;
  309. etatCamera.getPos = 0;
  310. rt_mutex_release(&mutex_etatCamera);
  311. camera.Close();
  312. /* if (err = rt_task_delete(&th_server)) {
  313. cerr << "Error task delete: " << strerror(-err) << endl << flush;
  314. //exit(EXIT_FAILURE);
  315. } */
  316. if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) {
  317. cerr << "Error task create: " << strerror(-err) << endl << flush;
  318. exit(EXIT_FAILURE);
  319. }
  320. if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
  321. cerr << "Error task start: " << strerror(-err) << endl << flush;
  322. exit(EXIT_FAILURE);
  323. }
  324. rt_sem_v(&sem_barrier);
  325. Kill_StartWD();
  326. break;
  327. } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
  328. rt_sem_v(&sem_openComRobot);
  329. } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
  330. rt_sem_v(&sem_startRobot);
  331. } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
  332. msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
  333. msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
  334. msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
  335. msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
  336. rt_mutex_acquire(&mutex_move, TM_INFINITE);
  337. move = msgRcv->GetID();
  338. rt_mutex_release(&mutex_move);
  339. cout << "On est dans le message mouvement" << endl << flush;
  340. }
  341. else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)){
  342. rt_mutex_acquire(&mutex_watchdog, TM_INFINITE);
  343. watchdog = 1;
  344. rt_mutex_release(&mutex_watchdog);
  345. rt_sem_v(&sem_startRobot);
  346. cout << "On a start le robot avec Watchdog" << endl << flush;
  347. }
  348. else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
  349. rt_sem_v(&sem_startCAM);
  350. }
  351. else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){
  352. camera.Close();
  353. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  354. etatCamera.cameraStarted = 0;
  355. etatCamera.arena = Arena();
  356. etatCamera.getImg = 0;
  357. etatCamera.getPos = 0;
  358. etatCamera.askArena = 0;
  359. rt_mutex_release(&mutex_etatCamera);
  360. }
  361. else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
  362. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  363. etatCamera.askArena = 1;
  364. etatCamera.getImg = 0;
  365. rt_mutex_release(&mutex_etatCamera);
  366. }
  367. else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)){
  368. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  369. etatCamera.askArena = 0;
  370. etatCamera.getImg = 1;
  371. rt_mutex_release(&mutex_etatCamera);
  372. }
  373. else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){
  374. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  375. etatCamera.arena = Arena();
  376. etatCamera.askArena = 0;
  377. etatCamera.getImg = 1;
  378. rt_mutex_release(&mutex_etatCamera);
  379. }
  380. else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
  381. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  382. etatCamera.getPos = 1;
  383. rt_mutex_release(&mutex_etatCamera);
  384. }
  385. else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){
  386. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  387. etatCamera.getPos = 0;
  388. rt_mutex_release(&mutex_etatCamera);
  389. }
  390. delete(msgRcv); // mus be deleted manually, no consumer
  391. }
  392. }
  393. }
  394. /**
  395. * @brief Thread opening communication with the robot.
  396. */
  397. void Tasks::OpenComRobot(void *arg) {
  398. int status;
  399. int err;
  400. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  401. // Synchronization barrier (waiting that all tasks are starting)
  402. rt_sem_p(&sem_barrier, TM_INFINITE);
  403. /**************************************************************************************/
  404. /* The task openComRobot starts here */
  405. /**************************************************************************************/
  406. while (1) {
  407. rt_sem_p(&sem_openComRobot, TM_INFINITE);
  408. cout << "Open serial com (";
  409. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  410. status = robot.Open();
  411. rt_mutex_release(&mutex_robot);
  412. cout << status;
  413. cout << ")" << endl << flush;
  414. Message * msgSend;
  415. if (status < 0) {
  416. msgSend = new Message(MESSAGE_ANSWER_NACK);
  417. } else {
  418. msgSend = new Message(MESSAGE_ANSWER_ACK);
  419. }
  420. WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
  421. }
  422. }
  423. /**
  424. * @brief Thread starting the communication with the robot.
  425. */
  426. void Tasks::StartRobotTask(void *arg) {
  427. int etatWD;
  428. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  429. // Synchronization barrier (waiting that all tasks are starting)
  430. rt_sem_p(&sem_barrier, TM_INFINITE);
  431. /**************************************************************************************/
  432. /* The task startRobot starts here */
  433. /**************************************************************************************/
  434. while (1) {
  435. Message * msgSend;
  436. rt_sem_p(&sem_startRobot, TM_INFINITE);
  437. cout << "Start robot (";
  438. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  439. rt_mutex_acquire(&mutex_watchdog, TM_INFINITE);
  440. etatWD = watchdog;
  441. rt_mutex_release(&mutex_watchdog);
  442. cout << "Watchdog = " << etatWD << endl << flush;
  443. if (etatWD == 0){
  444. cout << "Dans le if without WD" << etatWD << endl << flush;
  445. msgSend = robot.Write(robot.StartWithoutWD());
  446. rt_mutex_release(&mutex_robot);
  447. cout << msgSend->GetID();
  448. cout << ")" << endl;
  449. cout << "Movement answer: " << msgSend->ToString() << endl << flush;
  450. WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
  451. if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
  452. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  453. robotStarted = 1;
  454. rt_mutex_release(&mutex_robotStarted);
  455. }
  456. }
  457. else{
  458. cout << "Dans le if with WD" << etatWD << endl << flush;
  459. msgSend = robot.Write(robot.StartWithWD());
  460. rt_mutex_release(&mutex_robot);
  461. cout << msgSend->GetID();
  462. cout << ")" << endl;
  463. cout << "Movement answer: " << msgSend->ToString() << endl << flush;
  464. WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
  465. if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
  466. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  467. robotStarted = 1;
  468. rt_mutex_release(&mutex_robotStarted);
  469. rt_sem_v(&sem_startWD);
  470. }
  471. }
  472. }
  473. }
  474. /**
  475. * @brief Thread handling control of the robot.
  476. */
  477. void Tasks::MoveTask(void *arg) {
  478. int rs;
  479. int cpMove;
  480. int previousMove = -1;
  481. int cpt = 0;
  482. Message * response;
  483. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  484. // Synchronization barrier (waiting that all tasks are starting)
  485. rt_sem_p(&sem_barrier, TM_INFINITE);
  486. /**************************************************************************************/
  487. /* The task starts here */
  488. /**************************************************************************************/
  489. rt_task_set_periodic(NULL, TM_NOW, 100000000);
  490. while (1) {
  491. rt_task_wait_period(NULL);
  492. cout << "Periodic movement update";
  493. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  494. rs = robotStarted;
  495. rt_mutex_release(&mutex_robotStarted);
  496. if (rs == 1) {
  497. if (!(previousMove == move)){
  498. rt_mutex_acquire(&mutex_move, TM_INFINITE);
  499. cpMove = move;
  500. rt_mutex_release(&mutex_move);
  501. cout << " move: " << cpMove;
  502. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  503. response = robot.Write(new Message((MessageID)cpMove));
  504. rt_mutex_release(&mutex_robot);
  505. if (response->CompareID(MESSAGE_ANSWER_ACK)){
  506. cpt = 0;
  507. }
  508. else{
  509. cpt++;
  510. if (cpt == 3){
  511. Message * respToMon = new Message(MESSAGE_ANSWER_COM_ERROR);
  512. WriteInQueue(&q_messageToMon,respToMon);
  513. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  514. robotStarted = 0;
  515. rt_mutex_release(&mutex_robotStarted);
  516. rt_mutex_acquire(&mutex_watchdog, TM_INFINITE);
  517. watchdog = 0;
  518. rt_mutex_release(&mutex_watchdog);
  519. Kill_StartWD();
  520. cpt = 0;
  521. }
  522. }
  523. previousMove = cpMove;
  524. }
  525. }
  526. cout << endl << flush;
  527. }
  528. }
  529. /**
  530. * @brief Kills and restart the thread watchdog
  531. */
  532. void Tasks::Kill_StartWD(){
  533. int err;
  534. if (err = rt_task_delete(&th_watchdog)) {
  535. cerr << "Error task delete: " << strerror(-err) << endl << flush;
  536. exit(EXIT_FAILURE);
  537. }
  538. if (err = rt_task_create(&th_watchdog, "th_watchdog", 0, PRIORITY_TWATCHDOG, 0)) {
  539. cerr << "Error task create: " << strerror(-err) << endl << flush;
  540. exit(EXIT_FAILURE);
  541. }
  542. if (err = rt_task_start(&th_watchdog, (void(*)(void*)) & Tasks::ReloadWatchdogTask, this)) {
  543. cerr << "Error task start: " << strerror(-err) << endl << flush;
  544. exit(EXIT_FAILURE);
  545. }
  546. rt_sem_broadcast(&sem_barrier);
  547. }
  548. /**
  549. * Write a message in a given queue
  550. * @param queue Queue identifier
  551. * @param msg Message to be stored
  552. */
  553. void Tasks::WriteInQueue(RT_QUEUE *queue, Message *msg) {
  554. int err;
  555. if ((err = rt_queue_write(queue, (const void *) &msg, sizeof ((const void *) &msg), Q_NORMAL)) < 0) {
  556. cerr << "Write in queue failed: " << strerror(-err) << endl << flush;
  557. throw std::runtime_error{"Error in write in queue"};
  558. }
  559. }
  560. /**
  561. * Read a message from a given queue, block if empty
  562. * @param queue Queue identifier
  563. * @return Message read
  564. */
  565. Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
  566. int err;
  567. Message *msg;
  568. if ((err = rt_queue_read(queue, &msg, sizeof ((void*) &msg), TM_INFINITE)) < 0) {
  569. cout << "Read in queue failed: " << strerror(-err) << endl << flush;
  570. throw std::runtime_error{"Error in read in queue"};
  571. }/** else {
  572. cout << "@msg :" << msg << endl << flush;
  573. } /**/
  574. return msg;
  575. }
  576. void Tasks::SendBatteryTask(void *arg){
  577. int rs;
  578. Message * responseBattery;
  579. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  580. // Synchronization barrier (waiting that all tasks are starting)
  581. rt_sem_p(&sem_barrier, TM_INFINITE);
  582. /**************************************************************************************/
  583. /* The task starts here */
  584. /**************************************************************************************/
  585. rt_task_set_periodic(NULL, TM_NOW, 500000000);
  586. while(1){
  587. rt_task_wait_period(NULL);
  588. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  589. rs = robotStarted;
  590. rt_mutex_release(&mutex_robotStarted);
  591. cout << "Periodic battery update before rs" << endl << flush;
  592. if (rs){
  593. cout << "Periodic battery update" << endl << flush;
  594. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  595. responseBattery = robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET));
  596. rt_mutex_release(&mutex_robot);
  597. cout << "Periodic battery update before comparing message" << endl << flush;
  598. if (responseBattery->CompareID(MESSAGE_ROBOT_BATTERY_LEVEL)){
  599. WriteInQueue(&q_messageToMon,responseBattery);
  600. }
  601. }
  602. cout << endl << flush;
  603. }
  604. }
  605. /**
  606. * @brief Sends the reload message to the Robot if started with watchdog
  607. */
  608. void Tasks::ReloadWatchdogTask(void *arg) {
  609. int rs;
  610. // Synchronization barrier (waiting that all tasks are starting)
  611. rt_sem_p(&sem_barrier, TM_INFINITE);
  612. //We wait for the signal startWd
  613. rt_sem_p(&sem_startWD, TM_INFINITE);
  614. cout << "thread WD started" << endl << flush;
  615. /**************************************************************************************/
  616. /* The task starts here */
  617. /**************************************************************************************/
  618. rt_task_set_periodic(NULL, TM_NOW, 1000000000);
  619. while(1){
  620. rt_task_wait_period(NULL);
  621. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  622. rs = robotStarted;
  623. rt_mutex_release(&mutex_robotStarted);
  624. if (rs){
  625. cout << "Periodic watchdog reload" << endl << flush;
  626. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  627. robot.Write(new Message(MESSAGE_ROBOT_RELOAD_WD));
  628. rt_mutex_release(&mutex_robot);
  629. }
  630. }
  631. }
  632. /**
  633. * @brief Starts the Camera and the aquisition of images
  634. */
  635. void Tasks::StartCamera(void * arg){
  636. bool resp;
  637. // Synchronization barrier (waiting that all tasks are starting)
  638. rt_sem_p(&sem_barrier, TM_INFINITE);
  639. while(1){
  640. //We wait for the signal startCam
  641. rt_sem_p(&sem_startCAM, TM_INFINITE);
  642. cout << "thread StartCam started" << endl << flush;
  643. Message * msg;
  644. resp = camera.Open();
  645. if (resp){
  646. cout << "thread StartCam ok" << endl << flush;
  647. msg = new Message(MESSAGE_ANSWER_ACK);
  648. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  649. etatCamera.cameraStarted = 1;
  650. etatCamera.getImg = 1;
  651. rt_mutex_release(&mutex_etatCamera);
  652. }
  653. else{
  654. msg = new Message(MESSAGE_ANSWER_NACK);
  655. }
  656. WriteInQueue(&q_messageToMon,msg);
  657. }
  658. }
  659. /**
  660. * @brief The nominal task for the Camera
  661. */
  662. void Tasks::CameraTask(void * arg){
  663. struct etatCamera_t ec;
  664. // Synchronization barrier (waiting that all tasks are starting)
  665. rt_sem_p(&sem_barrier, TM_INFINITE);
  666. cout << "thread Camera started" << endl << flush;
  667. /**************************************************************************************/
  668. /* The task starts here */
  669. /**************************************************************************************/
  670. rt_task_set_periodic(NULL, TM_NOW, 100000000);
  671. while(1){
  672. rt_task_wait_period(NULL);
  673. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  674. ec = etatCamera;
  675. rt_mutex_release(&mutex_etatCamera);
  676. if (ec.cameraStarted == 1){
  677. if (ec.getImg == 1){
  678. Img img = camera.Grab();
  679. if (ec.getPos == 1){
  680. std::list<Position> pos;
  681. pos = img.SearchRobot(ec.arena);
  682. if (pos.empty()){
  683. //Atention j'ai modifié img.h, classe Position attribut center dans le constructeur
  684. pos.push_front(Position());
  685. }
  686. else{
  687. img.DrawAllRobots(pos);
  688. }
  689. MessagePosition* msgPos = new MessagePosition(MESSAGE_CAM_POSITION, pos.front());
  690. WriteInQueue(&q_messageToMon,msgPos);
  691. }
  692. MessageImg * msgImg;
  693. msgImg = new MessageImg(MESSAGE_CAM_IMAGE, &img);
  694. //WriteInQueue(&q_messageToMon,msgImg);
  695. monitor.Write(msgImg);
  696. }
  697. else if (ec.askArena == 1){
  698. Img img = camera.Grab();
  699. ec.arena = img.SearchArena();
  700. if (!ec.arena.IsEmpty()){
  701. rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
  702. etatCamera.arena = ec.arena;
  703. etatCamera.askArena = 0;
  704. rt_mutex_release(&mutex_etatCamera);
  705. img.DrawArena(ec.arena);
  706. MessageImg * msgImg;
  707. msgImg = new MessageImg(MESSAGE_CAM_IMAGE, &img);
  708. monitor.Write(msgImg);
  709. //WriteInQueue(&q_messageToMon,msgImg);
  710. }
  711. }
  712. }
  713. }
  714. }