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 24KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616
  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_TBATTERY 19
  28. #define PRIORITY_TWATCHDOG 31
  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. cout << "Mutexes created successfully" << endl << flush;
  74. /**************************************************************************************/
  75. /* Semaphors creation */
  76. /**************************************************************************************/
  77. if (err = rt_sem_create(&sem_barrier, NULL, 0, S_FIFO)) {
  78. cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
  79. exit(EXIT_FAILURE);
  80. }
  81. if (err = rt_sem_create(&sem_openComRobot, NULL, 0, S_FIFO)) {
  82. cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
  83. exit(EXIT_FAILURE);
  84. }
  85. if (err = rt_sem_create(&sem_serverOk, 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_startRobot, 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_restartServer, 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_robotStopped, NULL, 0, S_FIFO)) {
  98. cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
  99. exit(EXIT_FAILURE);
  100. }
  101. cout << "Semaphores created successfully" << endl << flush;
  102. /**************************************************************************************/
  103. /* Tasks creation */
  104. /**************************************************************************************/
  105. if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) {
  106. cerr << "Error task create: " << strerror(-err) << endl << flush;
  107. exit(EXIT_FAILURE);
  108. }
  109. if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) {
  110. cerr << "Error task create: " << strerror(-err) << endl << flush;
  111. exit(EXIT_FAILURE);
  112. }
  113. if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) {
  114. cerr << "Error task create: " << strerror(-err) << endl << flush;
  115. exit(EXIT_FAILURE);
  116. }
  117. if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) {
  118. cerr << "Error task create: " << strerror(-err) << endl << flush;
  119. exit(EXIT_FAILURE);
  120. }
  121. if (err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0)) {
  122. cerr << "Error task create: " << strerror(-err) << endl << flush;
  123. exit(EXIT_FAILURE);
  124. }
  125. if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
  126. cerr << "Error task create: " << strerror(-err) << endl << flush;
  127. exit(EXIT_FAILURE);
  128. }
  129. if (err = rt_task_create(&th_getBattery, "th_getBattery", 0, PRIORITY_TBATTERY, 0)) {
  130. cerr << "Error task create: " << strerror(-err) << endl << flush;
  131. exit(EXIT_FAILURE);
  132. }
  133. if (err = rt_task_create(&th_watchDog, "th_watchDog", 0, PRIORITY_TWATCHDOG, 0)) {
  134. cerr << "Error task create: " << strerror(-err) << endl << flush;
  135. exit(EXIT_FAILURE);
  136. }
  137. cout << "Tasks created successfully" << endl << flush;
  138. /**************************************************************************************/
  139. /* Message queues creation */
  140. /**************************************************************************************/
  141. if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
  142. cerr << "Error msg queue create: " << strerror(-err) << endl << flush;
  143. exit(EXIT_FAILURE);
  144. }
  145. if ((err = rt_queue_create(&q_messageComRobot, "q_messageComRobot", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
  146. cerr << "Error messageComRobot queue create: " << strerror(-err) << endl << flush;
  147. exit(EXIT_FAILURE);
  148. }
  149. if ((err = rt_queue_create(&q_messageControlRobot, "q_messageControlRobot", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
  150. cerr << "Error messageControlRobot queue create: " << strerror(-err) << endl << flush;
  151. exit(EXIT_FAILURE);
  152. }
  153. if ((err = rt_queue_create(&q_messageControlCam, "q_messageControlCam", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
  154. cerr << "Error messageControlCam queue create: " << strerror(-err) << endl << flush;
  155. exit(EXIT_FAILURE);
  156. }
  157. cout << "Queues created successfully" << endl << flush;
  158. }
  159. /**
  160. * @brief Démarrage des tâches
  161. */
  162. void Tasks::Run() {
  163. rt_task_set_priority(NULL, T_LOPRIO);
  164. int err;
  165. if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
  166. cerr << "Error task start: " << strerror(-err) << endl << flush;
  167. exit(EXIT_FAILURE);
  168. }
  169. if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
  170. cerr << "Error task start: " << strerror(-err) << endl << flush;
  171. exit(EXIT_FAILURE);
  172. }
  173. if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) {
  174. cerr << "Error task start: " << strerror(-err) << endl << flush;
  175. exit(EXIT_FAILURE);
  176. }
  177. if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
  178. cerr << "Error task start: " << strerror(-err) << endl << flush;
  179. exit(EXIT_FAILURE);
  180. }
  181. if (err = rt_task_start(&th_startRobot, (void(*)(void*)) & Tasks::StartRobotTask, this)) {
  182. cerr << "Error task start: " << strerror(-err) << endl << flush;
  183. exit(EXIT_FAILURE);
  184. }
  185. if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) {
  186. cerr << "Error task start: " << strerror(-err) << endl << flush;
  187. exit(EXIT_FAILURE);
  188. }
  189. if (err = rt_task_start(&th_getBattery, (void(*)(void*)) & Tasks::ReadBattery, this)) {
  190. cerr << "Error task start: " << strerror(-err) << endl << flush;
  191. exit(EXIT_FAILURE);
  192. }
  193. if (err = rt_task_start(&th_watchDog, (void(*)(void*)) & Tasks::WatchDog, this)) {
  194. cerr << "Error task start: " << strerror(-err) << endl << flush;
  195. exit(EXIT_FAILURE);
  196. }
  197. cout << "Tasks launched" << endl << flush;
  198. }
  199. /**
  200. * @brief Arrêt des tâches
  201. */
  202. void Tasks::Stop() {
  203. monitor.Close();
  204. robot.Close();
  205. }
  206. /**
  207. */
  208. void Tasks::Join() {
  209. cout << "Tasks synchronized" << endl << flush;
  210. rt_sem_broadcast(&sem_barrier);
  211. pause();
  212. }
  213. /**
  214. * @brief Thread handling server communication with the monitor.
  215. */
  216. void Tasks::ServerTask(void *arg) {
  217. int status;
  218. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  219. // Synchronization barrier (waiting that all tasks are started)
  220. rt_sem_p(&sem_barrier, TM_INFINITE);
  221. /**************************************************************************************/
  222. /* The task server starts here */
  223. /**************************************************************************************/
  224. while(1){
  225. rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
  226. status = monitor.Open(SERVER_PORT);
  227. rt_mutex_release(&mutex_monitor);
  228. cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
  229. if (status < 0) throw std::runtime_error {
  230. "Unable to start server on port " + std::to_string(SERVER_PORT)
  231. };
  232. monitor.AcceptClient(); // Wait the monitor client
  233. cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
  234. rt_sem_broadcast(&sem_serverOk);
  235. rt_sem_p(&sem_restartServer, TM_INFINITE);
  236. rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
  237. monitor.Close();
  238. rt_mutex_release(&mutex_monitor);
  239. }
  240. }
  241. /**
  242. * @brief Thread sending data to monitor.
  243. */
  244. void Tasks::SendToMonTask(void* arg) {
  245. Message *msg;
  246. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  247. // Synchronization barrier (waiting that all tasks are starting)
  248. rt_sem_p(&sem_barrier, TM_INFINITE);
  249. /**************************************************************************************/
  250. /* The task sendToMon starts here */
  251. /**************************************************************************************/
  252. rt_sem_p(&sem_serverOk, TM_INFINITE);
  253. while (1) {
  254. cout << "wait msg to send" << endl << flush;
  255. msg = ReadInQueue(&q_messageToMon);
  256. cout << "Send msg to mon: " << msg->ToString() << endl << flush;
  257. rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
  258. monitor.Write(msg); // The message is deleted with the Write
  259. rt_mutex_release(&mutex_monitor);
  260. }
  261. }
  262. /**
  263. * @brief Thread receiving data from monitor.
  264. */
  265. void Tasks::ReceiveFromMonTask(void *arg) {
  266. Message *msgRcv;
  267. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  268. // Synchronization barrier (waiting that all tasks are starting)
  269. rt_sem_p(&sem_barrier, TM_INFINITE);
  270. /**************************************************************************************/
  271. /* The task receiveFromMon starts here */
  272. /**************************************************************************************/
  273. while(1){
  274. rt_sem_p(&sem_serverOk, TM_INFINITE);
  275. cout << "Received message from monitor activated" << endl << flush;
  276. while (1) {
  277. msgRcv = monitor.Read();
  278. cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
  279. if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
  280. delete(msgRcv);
  281. WriteInQueue(&q_messageControlRobot, new Message(MESSAGE_ROBOT_RESET));
  282. WriteInQueue(&q_messageComRobot, new Message(MESSAGE_ROBOT_COM_CLOSE));
  283. WriteInQueue(&q_messageControlCam, new Message(MESSAGE_CAM_CLOSE));
  284. rt_sem_v(&sem_restartServer); // A VERIFIER
  285. break;
  286. } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN) || msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) {
  287. WriteInQueue(&q_messageComRobot, msgRcv);
  288. //rt_sem_v(&sem_openComRobot);
  289. } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD) || msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
  290. WriteInQueue(&q_messageControlRobot, msgRcv);
  291. //rt_sem_v(&sem_startRobot);
  292. }
  293. else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
  294. msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
  295. msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
  296. msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
  297. msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {
  298. rt_mutex_acquire(&mutex_move, TM_INFINITE);
  299. move = msgRcv->GetID();
  300. rt_mutex_release(&mutex_move);
  301. }
  302. else if (msgRcv->CompareID(MESSAGE_CAM_OPEN) ||
  303. msgRcv->CompareID(MESSAGE_CAM_CLOSE) ||
  304. msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA) ||
  305. msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) ||
  306. msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM) ||
  307. msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START) ||
  308. msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP))
  309. {
  310. WriteInQueue(&q_messageControlCam, msgRcv);
  311. }
  312. delete(msgRcv); // mus be deleted manually, no consumer
  313. }
  314. }
  315. }
  316. /**
  317. * @brief Thread opening communication with the robot.
  318. */
  319. void Tasks::OpenComRobot(void *arg) {
  320. int status;
  321. int err;
  322. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  323. // Synchronization barrier (waiting that all tasks are starting)
  324. rt_sem_p(&sem_barrier, TM_INFINITE);
  325. /**************************************************************************************/
  326. /* The task openComRobot starts here */
  327. /**************************************************************************************/
  328. while (1) {
  329. Message * msg;
  330. msg=ReadInQueue(&q_messageComRobot);
  331. if(msg->CompareID(MESSAGE_ROBOT_COM_OPEN)){
  332. cout << "Open serial com (";
  333. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  334. status = robot.Open();
  335. rt_mutex_release(&mutex_robot);
  336. cout << status;
  337. cout << ")" << endl << flush;
  338. Message * msgSend;
  339. if (status < 0) {
  340. msgSend = new Message(MESSAGE_ANSWER_NACK);
  341. } else {
  342. msgSend = new Message(MESSAGE_ANSWER_ACK);
  343. }
  344. WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
  345. }else if(msg->CompareID(MESSAGE_ROBOT_COM_CLOSE)){
  346. rt_sem_p(&sem_robotStopped, TM_INFINITE);
  347. cout << "Close serial com (";
  348. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  349. status = robot.Close();
  350. rt_mutex_release(&mutex_robot);
  351. cout << status;
  352. cout << ")" << endl << flush;
  353. Message * msgSend;
  354. if (status < 0) {
  355. msgSend = new Message(MESSAGE_ANSWER_NACK);
  356. } else {
  357. msgSend = new Message(MESSAGE_ANSWER_ACK);
  358. }
  359. WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
  360. }
  361. //delete(msg);
  362. //rt_sem_p(&sem_openComRobot, TM_INFINITE);
  363. }
  364. }
  365. /**
  366. * @brief Thread starting the communication with the robot.
  367. */
  368. void Tasks::StartRobotTask(void *arg) {
  369. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  370. // Synchronization barrier (waiting that all tasks are starting)
  371. rt_sem_p(&sem_barrier, TM_INFINITE);
  372. /**************************************************************************************/
  373. /* The task startRobot starts here */
  374. /**************************************************************************************/
  375. while (1) {
  376. Message * tmp;
  377. Message * msgSend;
  378. tmp = ReadInQueue(&q_messageControlRobot);
  379. if (tmp -> CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)){
  380. cout << "Start robot without watchdog (";
  381. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  382. msgSend = robot.Write(robot.StartWithoutWD());
  383. rt_mutex_release(&mutex_robot);
  384. cout << msgSend->GetID();
  385. cout << ")" << endl;
  386. cout << "Movement answer: " << msgSend->ToString() << endl << flush;
  387. WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
  388. if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
  389. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  390. robotStarted = 1;
  391. rt_mutex_release(&mutex_robotStarted);
  392. }
  393. } else if (tmp -> CompareID(MESSAGE_ROBOT_START_WITH_WD)){
  394. cout << "Start robot with watchdog (";
  395. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  396. msgSend = robot.Write(robot.StartWithWD());
  397. rt_task_set_periodic(&th_watchDog,TM_NOW,1000000000);
  398. rt_mutex_release(&mutex_robot);
  399. cout << msgSend->GetID();
  400. cout << ")" << endl;
  401. cout << "Movement answer: " << msgSend->ToString() << endl << flush;
  402. WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
  403. if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
  404. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  405. robotStarted = 1;
  406. rt_mutex_release(&mutex_robotStarted);
  407. } else {
  408. rt_task_set_periodic(&th_watchDog,TM_NOW,0);
  409. }
  410. } else if (tmp -> CompareID(MESSAGE_ROBOT_RESET)){
  411. rt_task_set_periodic(&th_watchDog,TM_NOW,0);
  412. cout << "Stopping Robot (";
  413. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  414. robotStarted = 0;
  415. rt_mutex_release(&mutex_robotStarted);
  416. rt_task_sleep(100000000);
  417. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  418. msgSend = robot.Write(robot.Reset());
  419. rt_mutex_release(&mutex_robot);
  420. rt_mutex_acquire(&mutex_move, TM_INFINITE);
  421. move = MESSAGE_ROBOT_STOP;
  422. rt_mutex_release(&mutex_move);
  423. cout << msgSend->GetID();
  424. cout << ")" << endl;
  425. cout << "Movement answer: " << msgSend->ToString() << endl << flush;
  426. WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
  427. rt_sem_v(&sem_robotStopped);
  428. }
  429. }
  430. }
  431. void Tasks::WatchDog(void *arg) {
  432. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  433. // Synchronization barrier (waiting that all tasks are starting)
  434. rt_sem_p(&sem_barrier, TM_INFINITE);
  435. rt_task_set_periodic(NULL, TM_NOW, 0);
  436. while (1) {
  437. cout << "Realoading" << endl << flush;
  438. rt_task_wait_period(NULL);
  439. Message * msgSend;
  440. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  441. msgSend = robot.ReloadWD();
  442. robot.Write(msgSend);
  443. rt_mutex_release(&mutex_robot);
  444. }
  445. }
  446. /**
  447. * @brief Thread handling control of the robot.
  448. */
  449. void Tasks::MoveTask(void *arg) {
  450. int rs;
  451. int previousMove = MESSAGE_ROBOT_GO_FORWARD;
  452. int cpMove;
  453. int nb_error = 0;
  454. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  455. // Synchronization barrier (waiting that all tasks are starting)
  456. rt_sem_p(&sem_barrier, TM_INFINITE);
  457. /**************************************************************************************/
  458. /* The task starts here */
  459. /**************************************************************************************/
  460. rt_task_set_periodic(NULL, TM_NOW, 100000000);
  461. Message * msgSend;
  462. while (1) {
  463. rt_task_wait_period(NULL);
  464. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  465. rs = robotStarted;
  466. rt_mutex_release(&mutex_robotStarted);
  467. if (rs == 1) {
  468. cout << "Periodic movement update";
  469. rt_mutex_acquire(&mutex_move, TM_INFINITE);
  470. cpMove = move;
  471. rt_mutex_release(&mutex_move);
  472. if (cpMove != previousMove || cpMove != MESSAGE_ROBOT_STOP) {
  473. cout << " move: " << cpMove;
  474. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  475. msgSend = robot.Write(new Message((MessageID)cpMove));
  476. rt_mutex_release(&mutex_robot);
  477. if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) {
  478. nb_error = 0;
  479. previousMove = cpMove;
  480. } else {
  481. nb_error++;
  482. }
  483. if (nb_error > 2) {
  484. cout << "ERREUR ROBOT DETECTED";
  485. WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR));
  486. rt_mutex_acquire(&mutex_move, TM_INFINITE);
  487. move = MESSAGE_ROBOT_STOP;
  488. rt_mutex_release(&mutex_move);
  489. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  490. robotStarted = 0;
  491. rt_mutex_release(&mutex_robotStarted);
  492. rt_task_set_periodic(&th_watchDog,TM_NOW,0);
  493. }
  494. }
  495. cout << endl << flush;
  496. }
  497. }
  498. }
  499. /**
  500. * Write a message in a given queue
  501. * @param queue Queue identifier
  502. * @param msg Message to be stored
  503. */
  504. void Tasks::WriteInQueue(RT_QUEUE *queue, Message *msg) {
  505. int err;
  506. if ((err = rt_queue_write(queue, (const void *) &msg, sizeof ((const void *) &msg), Q_NORMAL)) < 0) {
  507. cerr << "Write in queue failed: " << strerror(-err) << endl << flush;
  508. throw std::runtime_error{"Error in write in queue"};
  509. }
  510. }
  511. /**
  512. * Read a message from a given queue, block if empty
  513. * @param queue Queue identifier
  514. * @return Message read
  515. */
  516. Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
  517. int err;
  518. Message *msg;
  519. if ((err = rt_queue_read(queue, &msg, sizeof ((void*) &msg), TM_INFINITE)) < 0) {
  520. cout << "Read in queue failed: " << strerror(-err) << endl << flush;
  521. throw std::runtime_error{"Error in read in queue"};
  522. }/** else {
  523. cout << "@msg :" << msg << endl << flush;
  524. } /**/
  525. return msg;
  526. }
  527. void Tasks::ReadBattery(void *arg){
  528. cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
  529. // Synchronization barrier (waiting that all tasks are starting)
  530. rt_sem_p(&sem_barrier, TM_INFINITE);
  531. /**************************************************************************************/
  532. /* The task starts here */
  533. /**************************************************************************************/
  534. rt_task_set_periodic(NULL, TM_NOW, 500000000);
  535. int rs;
  536. while (1) {
  537. rt_task_wait_period(NULL);
  538. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  539. rs = robotStarted;
  540. rt_mutex_release(&mutex_robotStarted);
  541. if (rs == 1) {
  542. cout << "Periodic battery get lvl" << endl << flush;
  543. rt_mutex_acquire(&mutex_robot, TM_INFINITE);
  544. Message *msg = robot.Write( robot.GetBattery() ) ;
  545. rt_mutex_release(&mutex_robot);
  546. if (msg->CompareID((MessageID)MESSAGE_ROBOT_BATTERY_LEVEL)) {
  547. rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
  548. monitor.Write(msg);
  549. rt_mutex_release(&mutex_monitor);
  550. }
  551. }
  552. }
  553. }