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

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265
  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. #ifndef __WITH_PTHREAD__
  19. // Déclaration des priorités des taches
  20. #define PRIORITY_TSERVER 30
  21. #define PRIORITY_TOPENCOMROBOT 20
  22. #define PRIORITY_TMOVE 10
  23. #define PRIORITY_TSENDTOMON 25
  24. #define PRIORITY_TRECEIVEFROMMON 22
  25. #define PRIORITY_TSTARTROBOT 20
  26. char mode_start;
  27. void write_in_queue(RT_QUEUE *, MessageToMon);
  28. void f_server(void *arg) {
  29. int err;
  30. /* INIT */
  31. RT_TASK_INFO info;
  32. rt_task_inquire(NULL, &info);
  33. printf("Init %s\n", info.name);
  34. rt_sem_p(&sem_barrier, TM_INFINITE);
  35. err=openServer(DEFAULT_SERVER_PORT);
  36. if (err < 0) {
  37. printf("Failed to start server: %s\n", strerror(-err));
  38. exit(EXIT_FAILURE);
  39. } else {
  40. #ifdef _WITH_TRACE_
  41. printf("%s: server started\n", info.name);
  42. #endif
  43. //Waiting for a client to connect
  44. err=acceptClient();
  45. if (err<0) {
  46. printf("Client accept failed: %s\n", strerror(-err));
  47. exit(EXIT_FAILURE);
  48. }
  49. #ifdef _WITH_TRACE_
  50. printf ("client connected: %d\n", err);
  51. printf ("Rock'n'roll baby !\n");
  52. #endif
  53. rt_sem_broadcast(&sem_serverOk);
  54. }
  55. }
  56. void f_sendToMon(void * arg) {
  57. int err;
  58. MessageToMon msg;
  59. /* INIT */
  60. RT_TASK_INFO info;
  61. rt_task_inquire(NULL, &info);
  62. printf("Init %s\n", info.name);
  63. rt_sem_p(&sem_barrier, TM_INFINITE);
  64. #ifdef _WITH_TRACE_
  65. printf("%s : waiting for sem_serverOk\n", info.name);
  66. #endif
  67. rt_sem_p(&sem_serverOk, TM_INFINITE);
  68. while (1) {
  69. #ifdef _WITH_TRACE_
  70. printf("%s : waiting for a message in queue\n", info.name);
  71. #endif
  72. if (rt_queue_read(&q_messageToMon, &msg, sizeof (MessageToRobot), TM_INFINITE) >= 0) {
  73. #ifdef _WITH_TRACE_
  74. printf("%s : message {%s,%s} in queue\n", info.name, msg.header, (char*)msg.data);
  75. #endif
  76. send_message_to_monitor(msg.header, msg.data);
  77. free_msgToMon_data(&msg);
  78. rt_queue_free(&q_messageToMon, &msg);
  79. } else {
  80. printf("Error msg queue write: %s\n", strerror(-err));
  81. }
  82. }
  83. }
  84. void f_receiveFromMon(void *arg) {
  85. MessageFromMon msg;
  86. int err;
  87. /* INIT */
  88. RT_TASK_INFO info;
  89. rt_task_inquire(NULL, &info);
  90. printf("Init %s\n", info.name);
  91. rt_sem_p(&sem_barrier, TM_INFINITE);
  92. #ifdef _WITH_TRACE_
  93. printf("%s : waiting for sem_serverOk\n", info.name);
  94. #endif
  95. rt_sem_p(&sem_serverOk, TM_INFINITE);
  96. do {
  97. #ifdef _WITH_TRACE_
  98. printf("%s : waiting for a message from monitor\n", info.name);
  99. #endif
  100. err = receive_message_from_monitor(msg.header, msg.data);
  101. #ifdef _WITH_TRACE_
  102. printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data);
  103. #endif
  104. if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) {
  105. if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot
  106. #ifdef _WITH_TRACE_
  107. printf("%s: message open Xbee communication\n", info.name);
  108. #endif
  109. rt_sem_v(&sem_openComRobot);
  110. }
  111. } else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) {
  112. if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot
  113. #ifdef _WITH_TRACE_
  114. printf("%s: message start robot\n", info.name);
  115. #endif
  116. rt_sem_v(&sem_startRobot);
  117. } else if ((msg.data[0] == DMB_GO_BACK)
  118. || (msg.data[0] == DMB_GO_FORWARD)
  119. || (msg.data[0] == DMB_GO_LEFT)
  120. || (msg.data[0] == DMB_GO_RIGHT)
  121. || (msg.data[0] == DMB_STOP_MOVE)) {
  122. rt_mutex_acquire(&mutex_move, TM_INFINITE);
  123. robotMove = msg.data[0];
  124. rt_mutex_release(&mutex_move);
  125. #ifdef _WITH_TRACE_
  126. printf("%s: message update movement with %c\n", info.name, robotMove);
  127. #endif
  128. }
  129. }
  130. } while (err > 0);
  131. }
  132. void f_openComRobot(void * arg) {
  133. int err;
  134. /* INIT */
  135. RT_TASK_INFO info;
  136. rt_task_inquire(NULL, &info);
  137. printf("Init %s\n", info.name);
  138. rt_sem_p(&sem_barrier, TM_INFINITE);
  139. while (1) {
  140. #ifdef _WITH_TRACE_
  141. printf("%s : Wait sem_openComRobot\n", info.name);
  142. #endif
  143. rt_sem_p(&sem_openComRobot, TM_INFINITE);
  144. #ifdef _WITH_TRACE_
  145. printf("%s : sem_openComRobot arrived => open communication robot\n", info.name);
  146. #endif
  147. err = open_communication_robot();
  148. if (err == 0) {
  149. #ifdef _WITH_TRACE_
  150. printf("%s : the communication is opened\n", info.name);
  151. #endif
  152. MessageToMon msg;
  153. set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
  154. write_in_queue(&q_messageToMon, msg);
  155. } else {
  156. MessageToMon msg;
  157. set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
  158. write_in_queue(&q_messageToMon, msg);
  159. }
  160. }
  161. }
  162. void f_startRobot(void * arg) {
  163. int err;
  164. /* INIT */
  165. RT_TASK_INFO info;
  166. rt_task_inquire(NULL, &info);
  167. printf("Init %s\n", info.name);
  168. rt_sem_p(&sem_barrier, TM_INFINITE);
  169. while (1) {
  170. #ifdef _WITH_TRACE_
  171. printf("%s : Wait sem_startRobot\n", info.name);
  172. #endif
  173. rt_sem_p(&sem_startRobot, TM_INFINITE);
  174. #ifdef _WITH_TRACE_
  175. printf("%s : sem_startRobot arrived => Start robot\n", info.name);
  176. #endif
  177. err = send_command_to_robot(DMB_START_WITHOUT_WD);
  178. if (err == 0) {
  179. #ifdef _WITH_TRACE_
  180. printf("%s : the robot is started\n", info.name);
  181. #endif
  182. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  183. robotStarted = 1;
  184. rt_mutex_release(&mutex_robotStarted);
  185. MessageToMon msg;
  186. set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
  187. write_in_queue(&q_messageToMon, msg);
  188. } else {
  189. MessageToMon msg;
  190. set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
  191. write_in_queue(&q_messageToMon, msg);
  192. }
  193. }
  194. }
  195. void f_move(void *arg) {
  196. /* INIT */
  197. RT_TASK_INFO info;
  198. rt_task_inquire(NULL, &info);
  199. printf("Init %s\n", info.name);
  200. rt_sem_p(&sem_barrier, TM_INFINITE);
  201. /* PERIODIC START */
  202. #ifdef _WITH_PERIODIC_TRACE_
  203. printf("%s: start period\n", info.name);
  204. #endif
  205. rt_task_set_periodic(NULL, TM_NOW, 100000000);
  206. while (1) {
  207. #ifdef _WITH_PERIODIC_TRACE_
  208. printf("%s: Wait period \n", info.name);
  209. #endif
  210. rt_task_wait_period(NULL);
  211. #ifdef _WITH_PERIODIC_TRACE_
  212. printf("%s: Periodic activation\n", info.name);
  213. printf("%s: move equals %c\n", info.name, robotMove);
  214. #endif
  215. rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
  216. if (robotStarted) {
  217. rt_mutex_acquire(&mutex_move, TM_INFINITE);
  218. send_command_to_robot(robotMove);
  219. rt_mutex_release(&mutex_move);
  220. #ifdef _WITH_TRACE_
  221. printf("%s: the movement %c was sent\n", info.name, robotMove);
  222. #endif
  223. }
  224. rt_mutex_release(&mutex_robotStarted);
  225. }
  226. }
  227. void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
  228. void *buff;
  229. buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon));
  230. memcpy(buff, &msg, sizeof (MessageToMon));
  231. rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
  232. }
  233. #endif // __WITH_PTHREAD__