123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265 |
- /*
- * Copyright (C) 2018 dimercur
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
- #include "tasks.h"
-
- #ifndef __WITH_PTHREAD__
-
- // Déclaration des priorités des taches
- #define PRIORITY_TSERVER 30
- #define PRIORITY_TOPENCOMROBOT 20
- #define PRIORITY_TMOVE 10
- #define PRIORITY_TSENDTOMON 25
- #define PRIORITY_TRECEIVEFROMMON 22
- #define PRIORITY_TSTARTROBOT 20
-
- char mode_start;
-
- void write_in_queue(RT_QUEUE *, MessageToMon);
-
- void f_server(void *arg) {
- int err;
- /* INIT */
- RT_TASK_INFO info;
- rt_task_inquire(NULL, &info);
- printf("Init %s\n", info.name);
- rt_sem_p(&sem_barrier, TM_INFINITE);
-
- err=openServer(DEFAULT_SERVER_PORT);
-
- if (err < 0) {
- printf("Failed to start server: %s\n", strerror(-err));
- exit(EXIT_FAILURE);
- } else {
- #ifdef _WITH_TRACE_
- printf("%s: server started\n", info.name);
- #endif
- //Waiting for a client to connect
- err=acceptClient();
-
- if (err<0) {
- printf("Client accept failed: %s\n", strerror(-err));
- exit(EXIT_FAILURE);
- }
-
- #ifdef _WITH_TRACE_
- printf ("client connected: %d\n", err);
- printf ("Rock'n'roll baby !\n");
- #endif
- rt_sem_broadcast(&sem_serverOk);
- }
- }
-
- void f_sendToMon(void * arg) {
- int err;
- MessageToMon msg;
-
- /* INIT */
- RT_TASK_INFO info;
- rt_task_inquire(NULL, &info);
- printf("Init %s\n", info.name);
- rt_sem_p(&sem_barrier, TM_INFINITE);
-
- #ifdef _WITH_TRACE_
- printf("%s : waiting for sem_serverOk\n", info.name);
- #endif
- rt_sem_p(&sem_serverOk, TM_INFINITE);
- while (1) {
-
- #ifdef _WITH_TRACE_
- printf("%s : waiting for a message in queue\n", info.name);
- #endif
- if (rt_queue_read(&q_messageToMon, &msg, sizeof (MessageToRobot), TM_INFINITE) >= 0) {
- #ifdef _WITH_TRACE_
- printf("%s : message {%s,%s} in queue\n", info.name, msg.header, (char*)msg.data);
- #endif
-
- send_message_to_monitor(msg.header, msg.data);
- free_msgToMon_data(&msg);
- rt_queue_free(&q_messageToMon, &msg);
- } else {
- printf("Error msg queue write: %s\n", strerror(-err));
- }
- }
- }
-
- void f_receiveFromMon(void *arg) {
- MessageFromMon msg;
- int err;
-
- /* INIT */
- RT_TASK_INFO info;
- rt_task_inquire(NULL, &info);
- printf("Init %s\n", info.name);
- rt_sem_p(&sem_barrier, TM_INFINITE);
-
- #ifdef _WITH_TRACE_
- printf("%s : waiting for sem_serverOk\n", info.name);
- #endif
- rt_sem_p(&sem_serverOk, TM_INFINITE);
- do {
- #ifdef _WITH_TRACE_
- printf("%s : waiting for a message from monitor\n", info.name);
- #endif
- err = receive_message_from_monitor(msg.header, msg.data);
- #ifdef _WITH_TRACE_
- printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data);
- #endif
- if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) {
- if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot
- #ifdef _WITH_TRACE_
- printf("%s: message open Xbee communication\n", info.name);
- #endif
- rt_sem_v(&sem_openComRobot);
- }
- } else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) {
- if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot
- #ifdef _WITH_TRACE_
- printf("%s: message start robot\n", info.name);
- #endif
- rt_sem_v(&sem_startRobot);
-
- } else if ((msg.data[0] == DMB_GO_BACK)
- || (msg.data[0] == DMB_GO_FORWARD)
- || (msg.data[0] == DMB_GO_LEFT)
- || (msg.data[0] == DMB_GO_RIGHT)
- || (msg.data[0] == DMB_STOP_MOVE)) {
-
- rt_mutex_acquire(&mutex_move, TM_INFINITE);
- robotMove = msg.data[0];
- rt_mutex_release(&mutex_move);
- #ifdef _WITH_TRACE_
- printf("%s: message update movement with %c\n", info.name, robotMove);
- #endif
-
- }
- }
- } while (err > 0);
-
- }
-
- void f_openComRobot(void * arg) {
- int err;
-
- /* INIT */
- RT_TASK_INFO info;
- rt_task_inquire(NULL, &info);
- printf("Init %s\n", info.name);
- rt_sem_p(&sem_barrier, TM_INFINITE);
-
- while (1) {
- #ifdef _WITH_TRACE_
- printf("%s : Wait sem_openComRobot\n", info.name);
- #endif
- rt_sem_p(&sem_openComRobot, TM_INFINITE);
- #ifdef _WITH_TRACE_
- printf("%s : sem_openComRobot arrived => open communication robot\n", info.name);
- #endif
- err = open_communication_robot();
- if (err == 0) {
- #ifdef _WITH_TRACE_
- printf("%s : the communication is opened\n", info.name);
- #endif
- MessageToMon msg;
- set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
- write_in_queue(&q_messageToMon, msg);
- } else {
- MessageToMon msg;
- set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
- write_in_queue(&q_messageToMon, msg);
- }
- }
- }
-
- void f_startRobot(void * arg) {
- int err;
-
- /* INIT */
- RT_TASK_INFO info;
- rt_task_inquire(NULL, &info);
- printf("Init %s\n", info.name);
- rt_sem_p(&sem_barrier, TM_INFINITE);
-
- while (1) {
- #ifdef _WITH_TRACE_
- printf("%s : Wait sem_startRobot\n", info.name);
- #endif
- rt_sem_p(&sem_startRobot, TM_INFINITE);
- #ifdef _WITH_TRACE_
- printf("%s : sem_startRobot arrived => Start robot\n", info.name);
- #endif
- err = send_command_to_robot(DMB_START_WITHOUT_WD);
- if (err == 0) {
- #ifdef _WITH_TRACE_
- printf("%s : the robot is started\n", info.name);
- #endif
- rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
- robotStarted = 1;
- rt_mutex_release(&mutex_robotStarted);
- MessageToMon msg;
- set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
- write_in_queue(&q_messageToMon, msg);
- } else {
- MessageToMon msg;
- set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
- write_in_queue(&q_messageToMon, msg);
- }
- }
- }
-
- void f_move(void *arg) {
- /* INIT */
- RT_TASK_INFO info;
- rt_task_inquire(NULL, &info);
- printf("Init %s\n", info.name);
- rt_sem_p(&sem_barrier, TM_INFINITE);
-
- /* PERIODIC START */
- #ifdef _WITH_PERIODIC_TRACE_
- printf("%s: start period\n", info.name);
- #endif
- rt_task_set_periodic(NULL, TM_NOW, 100000000);
- while (1) {
- #ifdef _WITH_PERIODIC_TRACE_
- printf("%s: Wait period \n", info.name);
- #endif
- rt_task_wait_period(NULL);
- #ifdef _WITH_PERIODIC_TRACE_
- printf("%s: Periodic activation\n", info.name);
- printf("%s: move equals %c\n", info.name, robotMove);
- #endif
- rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
- if (robotStarted) {
- rt_mutex_acquire(&mutex_move, TM_INFINITE);
- send_command_to_robot(robotMove);
- rt_mutex_release(&mutex_move);
- #ifdef _WITH_TRACE_
- printf("%s: the movement %c was sent\n", info.name, robotMove);
- #endif
- }
- rt_mutex_release(&mutex_robotStarted);
- }
- }
-
- void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
- void *buff;
- buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon));
- memcpy(buff, &msg, sizeof (MessageToMon));
- rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
- }
-
- #endif // __WITH_PTHREAD__
|